static int mpu6500_backup_register(struct inv_mpu_state *st)
{
int result = 0;
result =
inv_i2c_read(st, MPUREG_PWR_MGMT_1,
2, mpu6500_selftest.pwm_mgmt);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_CONFIG,
1, &mpu6500_selftest.config);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_GYRO_CONFIG,
1, &mpu6500_selftest.gyro_config);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_ACCEL_CONFIG,
1, &mpu6500_selftest.accel_config);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_ACCEL_CONFIG2,
1, &mpu6500_selftest.accel_config2);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_USER_CTRL,
1, &mpu6500_selftest.user_ctrl);
if (result)
return result;
result = inv_i2c_read(st, MPUREG_FIFO_EN,
1, &mpu6500_selftest.fifo_enable);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_INT_ENABLE,
1, &mpu6500_selftest.int_enable);
if (result)
return result;
result =
inv_i2c_read(st, MPUREG_SMPLRT_DIV,
1, &mpu6500_selftest.smplrt_div);
if (result)
return result;
return result;
}
static int mpu6500_gyro_self_test(struct inv_mpu_state *st,
int *reg_avg, int *st_avg, int *ratio)
{
int result;
int ret_val;
int ct_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
u8 regs[3];
ret_val = 0;
result = inv_i2c_read(st, MPUREG_SELF_TEST_X_GYRO, 3, regs);
if (result)
return result;
for (i = 0; i < 3; i++) {
if (regs[i] != 0)
ct_shift_prod[i] = gyro_6500_st_tb[regs[i] - 1];
else
ct_shift_prod[i] = 0;
}
pr_info("reg_bias : %d, %d, %d \n", reg_avg[0], reg_avg[1], reg_avg[2]);
pr_info("st_avg : %d, %d, %d \n", st_avg[0], st_avg[1], st_avg[2]);
for (i = 0; i < 3; i++) {
st_shift_cust[i] = abs(reg_avg[i] - st_avg[i]);
if (ct_shift_prod[i]) {
st_shift_ratio[i] =
abs((st_shift_cust[i] / ct_shift_prod[i])
- DEF_ST_PRECISION);
ratio[i] = st_shift_ratio[i];
if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
ret_val |= 1 << i;
} else {
if (st_shift_cust[i] <
DEF_ST_PRECISION * DEF_GYRO_CT_SHIFT_MIN *
DEF_SELFTEST_GYRO_SENS)
ret_val |= 1 << i;
if (st_shift_cust[i] >
DEF_ST_PRECISION * DEF_GYRO_CT_SHIFT_MAX *
DEF_SELFTEST_GYRO_SENS)
ret_val |= 1 << i;
}
}
pr_info("ct_shift_prod : %d %d %d\n", ct_shift_prod[0],
ct_shift_prod[1], ct_shift_prod[2]);
pr_info("st_shift_cust : %d %d %d\n", st_shift_cust[0],
st_shift_cust[1], st_shift_cust[2]);
pr_info("st_shift_ratio : %d %d %d\n", st_shift_ratio[0],
st_shift_ratio[1], st_shift_ratio[2]);
pr_err("%s, ret_val = %d\n", __func__, ret_val);
return ret_val;
}
int inv_serial_read_fifo(
void *sl_handle,
unsigned char slave_addr,
unsigned short length,
unsigned char *data)
{
int result;
unsigned short bytes_read = 0;
if (length > FIFO_HW_SIZE) {
printk(KERN_ERR
"maximum fifo read length is %d\n", FIFO_HW_SIZE);
return INV_ERROR_INVALID_PARAMETER;
}
while (bytes_read < length) {
unsigned short this_len =
min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
result = inv_i2c_read((struct i2c_adapter *)sl_handle,
slave_addr, MPUREG_FIFO_R_W, this_len,
&data[bytes_read]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
bytes_read += this_len;
}
return 0;
}
int inv_serial_read(
void *sl_handle,
unsigned char slave_addr,
unsigned char register_addr,
unsigned short length,
unsigned char *data)
{
int result;
unsigned short bytes_read = 0;
if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
&& (register_addr == MPUREG_FIFO_R_W ||
register_addr == MPUREG_MEM_R_W)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
return INV_ERROR_INVALID_PARAMETER;
}
while (bytes_read < length) {
unsigned short this_len =
min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
result = inv_i2c_read((struct i2c_adapter *)sl_handle,
slave_addr, register_addr + bytes_read,
this_len, &data[bytes_read]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
bytes_read += this_len;
}
return 0;
}
int set_3050_bypass(struct inv_mpu_iio_s *st, bool enable)
{
struct inv_reg_map_s *reg;
int result;
u8 b;
reg = &st->reg;
result = inv_i2c_read(st, reg->user_ctrl, 1, &b);
if (result)
return result;
if (((b & BIT_3050_AUX_IF_EN) == 0) && enable)
return 0;
if ((b & BIT_3050_AUX_IF_EN) && (enable == 0))
return 0;
b &= ~BIT_3050_AUX_IF_EN;
if (!enable) {
b |= BIT_3050_AUX_IF_EN;
result = inv_i2c_single_write(st, reg->user_ctrl, b);
return result;
} else {
/* Coming out of I2C is tricky due to several erratta. Do not
* modify this algorithm
*/
/*
* 1) wait for the right time and send the command to change
* the aux i2c slave address to an invalid address that will
* get nack'ed
*
* 0x00 is broadcast. 0x7F is unlikely to be used by any aux.
*/
result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR,
MPU3050_BOGUS_ADDR);
if (result)
return result;
/*
* 2) wait enough time for a nack to occur, then go into
* bypass mode:
*/
usleep_range(MPU3050_NACK_MIN_TIME, MPU3050_NACK_MAX_TIME);
result = inv_i2c_single_write(st, reg->user_ctrl, b);
if (result)
return result;
/*
* 3) wait for up to one MPU cycle then restore the slave
* address
*/
msleep(MPU3050_ONE_MPU_TIME);
result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR,
st->plat_data.secondary_i2c_addr);
if (result)
return result;
result = inv_i2c_single_write(st, reg->user_ctrl, b);
if (result)
return result;
usleep_range(MPU3050_NACK_MIN_TIME, MPU3050_NACK_MAX_TIME);
}
return 0;
}
/**
* inv_init_config_mpu3050() - Initialize hardware, disable FIFO.
* @st: Device driver instance.
* Initial configuration:
* FSR: +/- 2000DPS
* DLPF: 42Hz
* FIFO rate: 50Hz
* Clock source: Gyro PLL
*/
int inv_init_config_mpu3050(struct iio_dev *indio_dev)
{
struct inv_reg_map_s *reg;
int result;
u8 data;
struct inv_mpu_iio_s *st = iio_priv(indio_dev);
if (st->chip_config.is_asleep)
return -EPERM;
/*reading AUX VDDIO register */
result = inv_i2c_read(st, REG_3050_AUX_VDDIO, 1, &data);
if (result)
return result;
data &= ~BIT_3050_VDDIO;
if (st->plat_data.level_shifter)
data |= BIT_3050_VDDIO;
result = inv_i2c_single_write(st, REG_3050_AUX_VDDIO, data);
if (result)
return result;
reg = &st->reg;
result = set_inv_enable(indio_dev, false);
if (result)
return result;
/*2000dps full scale range*/
result = inv_i2c_single_write(st, reg->lpf,
(INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT)
| INV_FILTER_42HZ);
if (result)
return result;
st->chip_config.fsr = INV_FSR_2000DPS;
st->chip_config.lpf = INV_FILTER_42HZ;
result = inv_i2c_single_write(st, reg->sample_rate_div,
ONE_K_HZ/INIT_FIFO_RATE - 1);
if (result)
return result;
st->chip_config.fifo_rate = INIT_FIFO_RATE;
st->irq_dur_ns = INIT_DUR_TIME;
st->chip_config.prog_start_addr = DMP_START_ADDR;
st->chip_config.gyro_enable = 1;
st->chip_config.gyro_fifo_enable = 1;
if ((SECONDARY_SLAVE_TYPE_ACCEL == st->plat_data.sec_slave_type) &&
st->mpu_slave) {
result = st->mpu_slave->setup(st);
if (result)
return result;
result = st->mpu_slave->set_fs(st, INV_FS_02G);
if (result)
return result;
result = st->mpu_slave->set_lpf(st, INIT_FIFO_RATE);
if (result)
return result;
st->chip_config.accl_enable = 1;
st->chip_config.accl_fifo_enable = 1;
}
return 0;
}
int invdmp_read_register(u16 addr, u16 length, u8 * data, void *dbase_data)
{
struct dmp_ctrl_t *dctl = dbase_data;
INV_DBG_FUNC_NAME;
return inv_i2c_read(dctl->i2c_handle, dctl->i2c_addr,
addr, length, data);
}
static int mpu6500_accel_self_test(struct inv_mpu_state *st,
int *reg_avg, int *st_avg, int *ratio)
{
int result;
int ret_val;
int ct_shift_prod[3], st_shift_cust[3], st_shift_ratio[3];
int i;
u8 regs[3];
#define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \
/ DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
#define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \
/ DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
ret_val = 0;
result = inv_i2c_read(st,
MPUREG_SELF_TEST_X_ACCEL, 3, regs);
if (result)
return result;
for (i = 0; i < 3; i++) {
if (regs[i] != 0)
ct_shift_prod[i] = gyro_6500_st_tb[regs[i] - 1];
else
ct_shift_prod[i] = 0;
}
pr_info("reg_bias : %d, %d, %d \n", reg_avg[0], reg_avg[1], reg_avg[2]);
pr_info("st_avg : %d, %d, %d \n", st_avg[0], st_avg[1], st_avg[2]);
for (i = 0; i < 3; i++) {
st_shift_cust[i] = abs(reg_avg[i] - st_avg[i]);
if (ct_shift_prod[i]) {
st_shift_ratio[i] =
abs(st_shift_cust[i] / ct_shift_prod[i]
- DEF_ST_PRECISION);
ratio[i] = st_shift_ratio[i];
if (st_shift_ratio[i] > DEF_6500_ACCEL_ST_SHIFT_DELTA)
ret_val |= 1 << i;
} else {
if (st_shift_cust[i] < ACCEL_ST_AL_MIN)
ret_val |= 1 << i;
if (st_shift_cust[i] > ACCEL_ST_AL_MAX)
ret_val |= 1 << i;
}
}
pr_info("ct_shift_prod : %d %d %d\n",
ct_shift_prod[0], ct_shift_prod[1], ct_shift_prod[2]);
pr_info("st_shift_cust : %d %d %d\n",
st_shift_cust[0], st_shift_cust[1], st_shift_cust[2]);
pr_info("st_shift_ratio : %d %d %d\n",
st_shift_ratio[0], st_shift_ratio[1], st_shift_ratio[2]);
return ret_val;
}
static int mpu6500_do_powerup(struct inv_mpu_state *st)
{
int result = 0;
char reg;
inv_i2c_single_write(st, MPUREG_PWR_MGMT_1, 0x1);
mdelay(20);
inv_i2c_read(st, MPUREG_PWR_MGMT_2, 1, ®);
reg &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG |\
BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
inv_i2c_single_write(st, MPUREG_PWR_MGMT_2, reg);
return result;
}
static int mpu6500_do_test(struct inv_mpu_state *st, int self_test_flag,
int *gyro_result, int *accel_result, int sensors)
{
int result, i, j, k, packet_size;
u8 data[BYTES_PER_SENSOR * 2], d;
int fifo_count, packet_count, ind, s;
if ((sensors & MPU6500_HWST_ALL) == MPU6500_HWST_ALL)
packet_size = BYTES_PER_SENSOR * 2;
else
packet_size = BYTES_PER_SENSOR;
result = inv_i2c_single_write(st, MPUREG_INT_ENABLE, 0);
if (result)
return result;
/* disable the sensor output to FIFO */
result = inv_i2c_single_write(st, MPUREG_FIFO_EN, 0);
if (result)
return result;
/* disable fifo reading */
result = inv_i2c_single_write(st, MPUREG_USER_CTRL, 0);
if (result)
return result;
/* clear FIFO */
result = inv_i2c_single_write(st, MPUREG_USER_CTRL, BIT_FIFO_RST);
if (result)
return result;
/* setup parameters */
result = inv_i2c_single_write(st, MPUREG_CONFIG, MPU_FILTER_184HZ);
if (result)
return result;
result = inv_i2c_single_write(st, MPUREG_ACCEL_CONFIG2,
DEF_ST_MPU6500_ACCEL_LPF);
if (result)
return result;
result = inv_i2c_single_write(st, MPUREG_SMPLRT_DIV, 0x0);
if (result)
return result;
result = inv_i2c_single_write(st, MPUREG_GYRO_CONFIG, self_test_flag | (MPU_FS_250DPS << 3));
if (result)
return result;
result = inv_i2c_single_write(st, MPUREG_ACCEL_CONFIG,
self_test_flag | DEF_SELFTEST_6500_ACCEL_FS);
if (result)
return result;
/* wait for the output to get stable */
mdelay(DEF_ST_STABLE_TIME);
/* enable FIFO reading */
result = inv_i2c_single_write(st,
MPUREG_USER_CTRL, BIT_FIFO_EN);
if (result)
return result;
/* enable sensor output to FIFO */
d = 0;
if (sensors & MPU6500_HWST_ACCEL)
d |= BITS_ACCEL_OUT;
if (sensors & MPU6500_HWST_GYRO)
d |= BITS_GYRO_OUT;
result = inv_i2c_single_write(st, MPUREG_FIFO_EN, d);
if (result)
return result;
for (i = 0; i < THREE_AXIS; i++) {
gyro_result[i] = 0;
accel_result[i] = 0;
}
s = 0;
while (s < INIT_SELFTEST_SAMPLES) {
mdelay(DEF_GYRO_WAIT_TIME);
/* stop sending data to FIFO */
result = inv_i2c_single_write(st, MPUREG_FIFO_EN, 0);
if (result)
return result;
result = inv_i2c_read(st,
MPUREG_FIFO_COUNTH, FIFO_COUNT_BYTE, data);
if (result)
return result;
fifo_count = be16_to_cpup((__be16 *)(&data[0]));
packet_count = fifo_count / packet_size;
for(k = 0 ; k < 5 ; k++)
{
result = inv_i2c_read(st, MPUREG_FIFO_R_W,
packet_size, data);
if (result)
return result;
}
if( packet_count < (INIT_SELFTEST_SAMPLES - 3)) {
printk(KERN_INFO "HW_SELF_TEST_PACKET_ERROR=%d", packet_count);
return -1;
}
i = 0;
while ((i < packet_count) && (s < INIT_SELFTEST_SAMPLES)) {
result = inv_i2c_read(st, MPUREG_FIFO_R_W,
packet_size, data);
if (result)
return result;
ind = 0;
if (sensors & MPU6500_HWST_ACCEL) {
for (j = 0; j < THREE_AXIS; j++)
accel_result[j] +=
(short)be16_to_cpup((__be16 *)(&data[2 * j]));
ind += BYTES_PER_SENSOR;
}
if (sensors & MPU6500_HWST_GYRO)
for (j = 0; j < THREE_AXIS; j++)
gyro_result[j] +=
(short)be16_to_cpup((__be16 *)(&data[ind + 2 * j]));
s++;
i++;
}
}
for (j = 0; j < THREE_AXIS; j++) {
gyro_result[j] = gyro_result[j]/s;
gyro_result[j] *= DEF_ST_PRECISION;
accel_result[j] = accel_result[j]/s;
accel_result[j] *= DEF_ST_PRECISION;
}
return 0;
}
int mpu6500_selftest_run(struct inv_mpu_state *st,
int packet_cnt[3],
int gyro_bias[3],
int gyro_rms[3],
int gyro_lsb_bias[3])
{
int ret_val = 0;
int result;
int packet_count;
long avg[3]={0};
long rms[3]={0};
int i, j;
unsigned char regs[7] = {0};
unsigned char data[FIFO_PACKET_SIZE * 2]={0}, read_data[2];
int gyro_data[3][GYRO_MAX_PACKET_THRESH]={{0},};
short fifo_cnt;
int gyro_avg_tmp[3]={0};
struct mpu6500_selftest_info test_setup = {
DEF_GYRO_SENS, DEF_GYRO_FULLSCALE, DEF_PACKET_THRESH,
DEF_TOTAL_TIMING_TOL, (int)DEF_BIAS_THRESH_SELF,
DEF_RMS_LSB_THRESH_SELF * DEF_RMS_LSB_THRESH_SELF,
/* now obsolete - has no effect */
DEF_TESTS_PER_AXIS, DEF_N_ACCEL_SAMPLES
};
char a_name[3][2] = { "X", "Y", "Z" };
/*backup registers */
result = mpu6500_backup_register(st);
if (result) {
pr_err("%s, register backup error=%d", __func__, result);
return result;
}
if (mpu6500_selftest.pwm_mgmt[0] & 0x60) {
result = inv_i2c_single_write(st, MPUREG_PWR_MGMT_1, 0x00);
if (result) {
pr_err("%s, init PWR_MGMT error=%d", __func__, result);
return result;
}
}
regs[0] = mpu6500_selftest.pwm_mgmt[1] & ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
result = inv_i2c_single_write(st, MPUREG_PWR_MGMT_2, regs[0]);
if (result) {
pr_err("%s, power mgmt setting error=%d", __func__, result);
return result;
}
result = inv_i2c_single_write(st, MPUREG_INT_ENABLE, 0);
if (result) {
pr_err("%s, INT ENABLE error=%d", __func__, result);
return result;
}
/* disable the sensor output to FIFO */
result = inv_i2c_single_write(st, MPUREG_FIFO_EN, 0);
if (result) {
pr_err("%s, FIFO EN error=%d", __func__, result);
return result;
}
/* make sure the DMP is disabled first */
result = inv_i2c_single_write(st, MPUREG_USER_CTRL, 0x00);
if (result) {
pr_err("%s, DMP disable error=%d", __func__, result);
return result;
}
/* clear FIFO */
result = inv_i2c_single_write(st, MPUREG_USER_CTRL, BIT_FIFO_RST);
if (result) {
pr_err("%s, USER_CTRL setting error=%d", __func__, result);
return result;
}
/* sample rate *//* = 1ms */
result = inv_i2c_single_write(st, MPUREG_SMPLRT_DIV, 0x00);
if (result) {
pr_err("%s, SMPLRT_DIV set error=%d", __func__, result);
return result;
}
test_setup.bias_thresh = DEF_BIAS_LSB_THRESH_SELF;
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_i2c_single_write(st, MPUREG_CONFIG, regs[0]);
if (result) {
pr_err("%s, CONFIG set error=%d", __func__, result);
return result;
}
switch (test_setup.gyro_fs) {
case 2000:
regs[0] = 0x03;
break;
case 1000:
regs[0] = 0x02;
break;
case 500:
regs[0] = 0x01;
break;
case 250:
default:
regs[0] = 0x00;
break;
}
result = inv_i2c_single_write(st, MPUREG_GYRO_CONFIG, regs[0] << 3);
if (result) {
pr_err("%s, GYRO_CONFIG set error=%d", __func__, result);
return result;
}
// Wait time
// msleep(GYRO_WAIT_TIME);
mdelay(200);
// Enable FIFO
result = inv_i2c_single_write(st, MPUREG_USER_CTRL, BIT_FIFO_EN);
if (result)
return result;
// Enable gyro output to FIFO
result = inv_i2c_single_write(st, MPUREG_FIFO_EN, BIT_GYRO_FIFO_EN);
if (result)
return result;
// Wait time
mdelay(GYRO_WAIT_TIME);
// Stop gyro FIFO
result = inv_i2c_single_write(st, MPUREG_FIFO_EN, BIT_FIFO_DIS);
if (result)
return result;
// Read FIFO count
result = inv_i2c_read(st, MPUREG_FIFO_COUNTH, 2, read_data);
if (result)
return result;
fifo_cnt = be16_to_cpup((__be16 *)(&read_data[0]));
packet_count = fifo_cnt != 0 ? (fifo_cnt/FIFO_PACKET_SIZE) : 1;
if(packet_count > GYRO_PACKET_THRESH)
packet_count = GYRO_PACKET_THRESH;
// Check packet count
if((abs(packet_count - GYRO_PACKET_THRESH) > GYRO_THRESH) && (packet_count < GYRO_PACKET_THRESH)) {
pr_info("\r\n Gyro Packet counter Error: %d \r\n",packet_count);
return (ret_val |= 1);
}
pr_info("\r\n Gyro Packet counter : %d \r\n",packet_count);
for (i = 0; i < packet_count; i++) {
/* getting FIFO data */
result = inv_i2c_read(st, MPUREG_FIFO_R_W, FIFO_PACKET_SIZE, data);
if (result)
return result;
for (j = 0; j < THREE_AXIS; j++) {
gyro_data[j][i] = (int)mpu_big8_to_int16((&data[2*j]));
gyro_avg_tmp[j] += gyro_data[j][i];
avg[j] = (long)gyro_avg_tmp[j];
avg[j] /= packet_count;
}
}
pr_info("bias : %+8ld %+8ld %+8ld (LSB)\n", avg[X], avg[Y], avg[Z]);
gyro_bias[X] = (int)((avg[X] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS);
gyro_bias[Y] = (int)((avg[Y] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS);
gyro_bias[Z] = (int)((avg[Z] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS);
gyro_lsb_bias[X] = (int)avg[X];
gyro_lsb_bias[Y] = (int)avg[Y];
gyro_lsb_bias[Z] = (int)avg[Z];
if (VERBOSE_OUT) {
pr_info("abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[X]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[X]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[Y]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[Y]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[Z]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[Z]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (abs(avg[j]) > test_setup.bias_thresh) {
pr_err("%s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\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, rms[Y] = 0, rms[Z] = 0; i < packet_count; i++) {
rms[X] += (long)(gyro_data[0][i] - avg[X]) * (gyro_data[0][i] - avg[X]);
rms[Y] += (long)(gyro_data[1][i] - avg[Y]) * (gyro_data[1][i] - avg[Y]);
rms[Z] += (long)(gyro_data[2][i] - avg[Z]) * (gyro_data[2][i] - avg[Z]);
if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0)
pr_err("RMS returns zero, gyro = %d %d %d (avg = %ld %ld %ld)\n",
gyro_data[0][i], gyro_data[1][i], gyro_data[2][i],
avg[X], avg[Y], avg[Z]);
}
if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[X] / packet_count,
(long)rms[Y] / packet_count, (long)rms[Z] / packet_count);
}
{
int dps_rms[3] = { 0 };
u32 tmp;
int i = 0;
for (j = 0; j < 3; j++) {
if (rms[j] / packet_count > test_setup.rms_thresh) {
pr_err("%s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / packet_count,
test_setup.rms_thresh);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
tmp = ((u32) (rms[i] / packet_count)) * DEF_RMS_SCALE_FOR_RMS;
} else {
tmp = ((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) / packet_count;
}
if (rms[i] < 0)
tmp = 1 << 31;
dps_rms[i] = mpu6500_selftest_sqrt(tmp) / DEF_GYRO_SENS;
gyro_rms[i] = dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[X]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[X]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[Y]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[Y]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[Z]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[Z]) % DEF_SCALE_FOR_FLOAT);
}
/*recover registers */
result = mpu6500_recover_register(st);
if (result) {
pr_err("%s, register recovering error=%d", __func__, result);
return result;
}
return ret_val;
}
