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; }