int mpu6050_selftest_run(struct i2c_client *client,
int packet_cnt[3],
int gyro_bias[3],
int gyro_rms[3], int gyro_lsb_bias[3])
{
int ret_val = 0;
int result;
int total_count = 0;
int total_count_axis[3] = { 0, 0, 0 };
int packet_count;
long avg[3];
long rms[3];
short *x;
short *y;
short *z;
int buf_size = sizeof(short) * DEF_PERIOD_CAL
* DEF_TESTS_PER_AXIS / 8 * 4;
int i, j, tmp;
char tmpStr[200];
unsigned char regs[7] = { 0, };
unsigned char dataout[20];
int perform_full_test = 0;
int dps_rms[3] = { 0, };
u32 temp;
struct mpu6050_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" };
x = kzalloc(buf_size, GFP_KERNEL);
y = kzalloc(buf_size, GFP_KERNEL);
z = kzalloc(buf_size, GFP_KERNEL);
/*backup registers */
result = mpu6050_backup_register(client);
if (result) {
pr_err("register backup error=%d", result);
goto err_state;
}
if (mpu6050_selftest.pwm_mgmt[0] & 0x40) {
result =
mpu6050_i2c_write_single_reg(client, MPUREG_PWR_MGMT_1,
0x00);
if (result) {
pr_err("init PWR_MGMT error=%d", result);
goto err_state;
}
}
regs[0] =
mpu6050_selftest.pwm_mgmt[1] &
~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
result =
mpu6050_i2c_write_single_reg(client,
MPUREG_PWR_MGMT_2, regs[0]);
/* make sure the DMP is disabled first */
result = mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL, 0x00);
if (result) {
pr_info("DMP disable error=%d", result);
goto err_state;
}
/* reset the gyro offset values */
regs[0] = MPUREG_XG_OFFS_USRH;
result = mpu6050_i2c_write(client, 6, regs);
if (result)
goto err_state;
/* sample rate */
if (perform_full_test) {
/* = 8ms */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_SMPLRT_DIV,
0x07);
test_setup.bias_thresh = DEF_BIAS_LSB_THRESH_CAL;
} else {
/* = 1ms */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_SMPLRT_DIV,
0x00);
test_setup.bias_thresh = DEF_BIAS_LSB_THRESH_SELF;
}
if (result)
goto err_state;
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 = mpu6050_i2c_write_single_reg(client, MPUREG_CONFIG, regs[0]);
if (result)
goto err_state;
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 =
mpu6050_i2c_write_single_reg(client, MPUREG_GYRO_CONFIG,
regs[0] << 3);
if (result)
goto err_state;
result = mpu6050_i2c_write_single_reg(client, MPUREG_INT_ENABLE, 0x00);
if (result)
goto err_state;
/* 1st, timing test */
for (j = 0; j < 3; j++) {
pr_info("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 =
mpu6050_i2c_write_single_reg(client, MPUREG_PWR_MGMT_1,
j + 1);
if (result)
goto err_state;
/* wait for 50 ms after switching clock source */
msleep(50);
/* enable & reset FIFO */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL,
BIT_FIFO_EN | BIT_FIFO_RST);
if (result)
goto err_state;
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 =
mpu6050_i2c_write_single_reg(client, fifo_en_reg,
BIT_GYRO_XOUT |
BIT_GYRO_YOUT |
BIT_GYRO_ZOUT);
if (result)
goto err_state;
pr_info("%s : before sampling ...\n", __func__);
/* wait one period for data */
if (perform_full_test)
msleep(DEF_PERIOD_CAL);
else
msleep(DEF_PERIOD_SELF);
pr_info("%s : after sampling ...\n", __func__);
/* stop storing gyro in the FIFO */
result =
mpu6050_i2c_write_single_reg(client,
fifo_en_reg, 0x00);
if (result)
goto err_state;
/* Getting number of bytes in FIFO */
result =
mpu6050_i2c_read_reg(client, MPUREG_FIFO_COUNTH,
2, dataout);
if (result)
goto err_state;
/* number of 6 B packets in the FIFO */
packet_count = mpu6050_big8_to_int16(dataout) / 6;
pr_info("%s : sampling result packet_count =%d\n",
__func__, packet_count);
sprintf(tmpStr, "Packet Count: %d -", packet_count);
if (packet_count - (test_setup.packet_thresh - 3)
> 0) {
for (i = 0; i < packet_count; i++) {
/* getting FIFO data */
result =
mpu6050_i2c_read_fifo(client, 6,
dataout);
if (result)
goto err_state;
x[total_count + i] =
mpu6050_big8_to_int16(&dataout[0]);
y[total_count + i] =
mpu6050_big8_to_int16(&dataout[2]);
z[total_count + i] =
mpu6050_big8_to_int16(&dataout[4]);
}
total_count += packet_count;
total_count_axis[j] += packet_count;
packet_cnt[j] = packet_count;
sprintf(tmpStr, "%sOK success", tmpStr);
} else {
ret_val |= 1 << j;
sprintf(tmpStr, "%sNOK - samples ignored ...",
tmpStr);
}
pr_info("%s\n", tmpStr);
}
/* remove gyros from FIFO */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, 0x00);
if (result)
goto err_state;
}
pr_info("\nTotal %d samples\n", total_count);
/* 2nd, check bias from X, Y, and Z PLL clock source */
tmp = total_count != 0 ? total_count : 1;
for (i = 0, avg[0] = 0, avg[1] = 0, avg[2] = 0; i < total_count; i++) {
avg[0] += x[i];
avg[1] += y[i];
avg[2] += z[i];
}
avg[0] /= tmp;
avg[1] /= tmp;
avg[2] /= tmp;
pr_info("bias : %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_lsb_bias[0] = avg[0];
gyro_lsb_bias[1] = avg[1];
gyro_lsb_bias[2] = avg[2];
if (VERBOSE_OUT) {
pr_info("abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (abs(avg[j]) > test_setup.bias_thresh) {
pr_info("%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[0] = 0, rms[1] = 0, rms[2] = 0; i < total_count; i++) {
rms[0] += (long)(x[i] - avg[0]) * (x[i] - avg[0]);
rms[1] += (long)(y[i] - avg[1]) * (y[i] - avg[1]);
rms[2] += (long)(z[i] - avg[2]) * (z[i] - avg[2]);
}
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (rms[j] / total_count > test_setup.rms_thresh) {
pr_info("%s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, test_setup.rms_thresh);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / 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[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
/*recover registers */
result = mpu6050_recover_register(client);
if (result) {
pr_err("register recovering error=%d", result);
goto err_state;
}
result = ret_val;
err_state:
kfree(x);
kfree(y);
kfree(z);
return result;
}
static ssize_t mpu6500_gyro_selftest(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[36] = { 0,};
u8 initialized = 0;
s8 hw_result = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 shift_ratio[3] = {0,};
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
int bias_thresh = DEF_BIAS_LSB_THRESH_SELF_6500;
struct ssp_data *data = dev_get_drvdata(dev);
struct ssp_msg *msg = kzalloc(sizeof(*msg), GFP_KERNEL);
if (msg == NULL) {
pr_err("[SSP] %s, failed to alloc memory for ssp_msg\n", __func__);
goto exit;
}
msg->cmd = GYROSCOPE_FACTORY;
msg->length = 36;
msg->options = AP2HUB_READ;
msg->buffer = chTempBuf;
msg->free_buffer = 0;
iRet = ssp_spi_sync(data, msg, 7000);
if (iRet != SUCCESS) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
ret_val = 1;
goto exit;
}
data->uTimeOutCnt = 0;
pr_err("[SSP]%d %d %d %d %d %d %d %d %d %d %d %d", chTempBuf[0],
chTempBuf[1], chTempBuf[2], chTempBuf[3], chTempBuf[4],
chTempBuf[5], chTempBuf[6], chTempBuf[7], chTempBuf[8],
chTempBuf[9], chTempBuf[10], chTempBuf[11]);
initialized = chTempBuf[0];
shift_ratio[0] = (s16)((chTempBuf[2] << 8) +
chTempBuf[1]);
shift_ratio[1] = (s16)((chTempBuf[4] << 8) +
chTempBuf[3]);
shift_ratio[2] = (s16)((chTempBuf[6] << 8) +
chTempBuf[5]);
hw_result = (s8)chTempBuf[7];
total_count = (int)((chTempBuf[11] << 24) +
(chTempBuf[10] << 16) +
(chTempBuf[9] << 8) +
chTempBuf[8]);
avg[0] = (long)((chTempBuf[15] << 24) +
(chTempBuf[14] << 16) +
(chTempBuf[13] << 8) +
chTempBuf[12]);
avg[1] = (long)((chTempBuf[19] << 24) +
(chTempBuf[18] << 16) +
(chTempBuf[17] << 8) +
chTempBuf[16]);
avg[2] = (long)((chTempBuf[23] << 24) +
(chTempBuf[22] << 16) +
(chTempBuf[21] << 8) +
chTempBuf[20]);
rms[0] = (long)((chTempBuf[27] << 24) +
(chTempBuf[26] << 16) +
(chTempBuf[25] << 8) +
chTempBuf[24]);
rms[1] = (long)((chTempBuf[31] << 24) +
(chTempBuf[30] << 16) +
(chTempBuf[29] << 8) +
chTempBuf[28]);
rms[2] = (long)((chTempBuf[35] << 24) +
(chTempBuf[34] << 16) +
(chTempBuf[33] << 8) +
chTempBuf[32]);
pr_info("[SSP] init: %d, total cnt: %d\n", initialized, total_count);
pr_info("[SSP] hw_result: %d, %d, %d, %d\n", hw_result,
shift_ratio[0], shift_ratio[1], shift_ratio[2]);
pr_info("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_info("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
if (total_count == 0) {
pr_err("[SSP] %s, total_count is 0. goto exit\n", __func__);
ret_val = 2;
goto exit;
}
if (hw_result < 0) {
pr_err("[SSP] %s - hw selftest fail(%d), sw selftest skip\n",
__func__, hw_result);
return sprintf(buf, "-1,0,0,0,0,0,0,%d.%d,%d.%d,%d.%d,0,0,0\n",
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10);
}
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (unlikely(abs(avg[j]) > bias_thresh)) {
pr_err("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], 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 */
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (unlikely(rms[j] / total_count > DEF_RMS_THRESH)) {
pr_err("[SSP] %s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, DEF_RMS_THRESH);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (likely(!ret_val)) {
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0, gyrocal is 0 at all axis\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
}
exit:
ssp_dbg("[SSP]: %s - %d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
__func__, ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
return sprintf(buf, "%d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
}
ssize_t mpu6500_gyro_selftest(char *buf, struct ssp_data *data)
{
char chTempBuf[2] = { 3, 200};
u8 initialized = 0;
s8 hw_result = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 shift_ratio[3] = {0,};
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iDelayCnt = 0, iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
int bias_thresh = DEF_BIAS_LSB_THRESH_SELF_6500;
data->uFactorydataReady = 0;
memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_FACTORY,
chTempBuf, 2);
while (!(data->uFactorydataReady & (1 << GYROSCOPE_FACTORY))
&& (iDelayCnt++ < 150)
&& (iRet == SUCCESS))
msleep(20);
if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
initialized = data->uFactorydata[0];
shift_ratio[0] = (s16)((data->uFactorydata[2] << 8) +
data->uFactorydata[1]);
shift_ratio[1] = (s16)((data->uFactorydata[4] << 8) +
data->uFactorydata[3]);
shift_ratio[2] = (s16)((data->uFactorydata[6] << 8) +
data->uFactorydata[5]);
hw_result = (s8)data->uFactorydata[7];
total_count = (int)((data->uFactorydata[11] << 24) +
(data->uFactorydata[10] << 16) +
(data->uFactorydata[9] << 8) +
data->uFactorydata[8]);
avg[0] = (long)((data->uFactorydata[15] << 24) +
(data->uFactorydata[14] << 16) +
(data->uFactorydata[13] << 8) +
data->uFactorydata[12]);
avg[1] = (long)((data->uFactorydata[19] << 24) +
(data->uFactorydata[18] << 16) +
(data->uFactorydata[17] << 8) +
data->uFactorydata[16]);
avg[2] = (long)((data->uFactorydata[23] << 24) +
(data->uFactorydata[22] << 16) +
(data->uFactorydata[21] << 8) +
data->uFactorydata[20]);
rms[0] = (long)((data->uFactorydata[27] << 24) +
(data->uFactorydata[26] << 16) +
(data->uFactorydata[25] << 8) +
data->uFactorydata[24]);
rms[1] = (long)((data->uFactorydata[31] << 24) +
(data->uFactorydata[30] << 16) +
(data->uFactorydata[29] << 8) +
data->uFactorydata[28]);
rms[2] = (long)((data->uFactorydata[35] << 24) +
(data->uFactorydata[34] << 16) +
(data->uFactorydata[33] << 8) +
data->uFactorydata[32]);
pr_info("[SSP] init: %d, total cnt: %d\n", initialized, total_count);
pr_info("[SSP] hw_result: %d, %d, %d, %d\n", hw_result,
shift_ratio[0], shift_ratio[1], shift_ratio[2]);
pr_info("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_info("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
if (hw_result < 0) {
pr_err("[SSP] %s - hw selftest fail(%d), sw selftest skip\n",
__func__, hw_result);
return sprintf(buf, "-1,0,0,0,0,0,0,%d.%d,%d.%d,%d.%d,0,0,0\n",
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10);
}
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
if (data->ap_rev < 3)
bias_thresh = DEF_BIAS_LSB_THRESH_SELF;
for (j = 0; j < 3; j++) {
if (unlikely(abs(avg[j]) > bias_thresh)) {
pr_err("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], 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 */
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (unlikely(rms[j] / total_count > DEF_RMS_THRESH)) {
pr_err("[SSP] %s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, DEF_RMS_THRESH);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (likely(!ret_val)) {
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0, gyrocal is 0 at all axis\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
}
exit:
ssp_dbg("[SSP]: %s - %d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
__func__, ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
return sprintf(buf, "%d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%d,%d.%d,%d.%d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
shift_ratio[0] / 10, shift_ratio[0] % 10,
shift_ratio[1] / 10, shift_ratio[1] % 10,
shift_ratio[2] / 10, shift_ratio[2] % 10,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
}
static ssize_t gyro_selftest_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
char chTempBuf[2] = { 3, 200};
u8 initialized = 0;
int i = 0, j = 0, total_count = 0, ret_val = 0;
long avg[3] = {0,}, rms[3] = {0,};
int gyro_bias[3] = {0,}, gyro_rms[3] = {0,};
s16 iCalData[3] = {0,};
char a_name[3][2] = { "X", "Y", "Z" };
int iDelayCnt = 0, iRet = 0;
int dps_rms[3] = { 0, };
u32 temp = 0;
struct ssp_data *data = dev_get_drvdata(dev);
data->uFactorydataReady = 0;
memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_FACTORY,
chTempBuf, 2);
while (!(data->uFactorydataReady & (1 << GYROSCOPE_FACTORY))
&& (iDelayCnt++ < 150)
&& (iRet == SUCCESS))
msleep(20);
if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
pr_err("[SSP]: %s - Gyro Selftest Timeout!!\n", __func__);
goto exit;
}
for (i = 0; i < 29; i++)
pr_err("[SSP]: %s - uFactorydata[%d] = 0x%x\n", __func__, i,
data->uFactorydata[i]);
initialized = data->uFactorydata[0];
total_count = (int)((data->uFactorydata[4] << 24) +
(data->uFactorydata[3] << 16) +
(data->uFactorydata[2] << 8) +
data->uFactorydata[1]);
avg[0] = (long)((data->uFactorydata[8] << 24) +
(data->uFactorydata[7] << 16) +
(data->uFactorydata[6] << 8) +
data->uFactorydata[5]);
avg[1] = (long)((data->uFactorydata[12] << 24) +
(data->uFactorydata[11] << 16) +
(data->uFactorydata[10] << 8) +
data->uFactorydata[9]);
avg[2] = (long)((data->uFactorydata[16] << 24) +
(data->uFactorydata[15] << 16) +
(data->uFactorydata[14] << 8) +
data->uFactorydata[13]);
rms[0] = (long)((data->uFactorydata[20] << 24) +
(data->uFactorydata[19] << 16) +
(data->uFactorydata[18] << 8) +
data->uFactorydata[17]);
rms[1] = (long)((data->uFactorydata[24] << 24) +
(data->uFactorydata[23] << 16) +
(data->uFactorydata[22] << 8) +
data->uFactorydata[21]);
rms[2] = (long)((data->uFactorydata[28] << 24) +
(data->uFactorydata[27] << 16) +
(data->uFactorydata[26] << 8) +
data->uFactorydata[25]);
pr_err("[SSP] init: %d, total cnt: %d", initialized, total_count);
pr_err("[SSP] avg %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
pr_err("[SSP] rms %+8ld %+8ld %+8ld (LSB)\n", rms[0], rms[1], rms[2]);
/*
avg[0] /= total_count;
avg[1] /= total_count;
avg[2] /= total_count;
*/
pr_info("[SSP] bias : %+8ld %+8ld %+8ld (LSB)\n",
avg[0], avg[1], avg[2]);
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
iCalData[0] = (s16)avg[0];
iCalData[1] = (s16)avg[1];
iCalData[2] = (s16)avg[2];
if (VERBOSE_OUT) {
pr_info("[SSP] abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (abs(avg[j]) > DEF_BIAS_LSB_THRESH_SELF) {
pr_info("[SSP] %s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], DEF_BIAS_LSB_THRESH_SELF);
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 */
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("[SSP] RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (rms[j] / total_count > DEF_RMS_THRESH) {
pr_info("[SSP] %s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, DEF_RMS_THRESH);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("[SSP] RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
if (!ret_val) {
pr_err("[SSP] ret_val == 0\n");
save_gyro_caldata(data, iCalData);
} else {
pr_err("[SSP] ret_val != 0\n");
data->gyrocal.x = 0;
data->gyrocal.y = 0;
data->gyrocal.z = 0;
/*initialized = -1;*/
}
exit:
ssp_dbg("[SSP]: Gyro Selftest - %d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
return sprintf(buf, "%d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d.%03d,%d.%03d,%d.%03d,"
"%d,%d,%d\n",
ret_val,
(int)abs(gyro_bias[0]/1000),
(int)abs(gyro_bias[0])%1000,
(int)abs(gyro_bias[1]/1000),
(int)abs(gyro_bias[1])%1000,
(int)abs(gyro_bias[2]/1000),
(int)abs(gyro_bias[2])%1000,
gyro_rms[0]/1000,
(int)abs(gyro_rms[0])%1000,
gyro_rms[1]/1000,
(int)abs(gyro_rms[1])%1000,
gyro_rms[2]/1000,
(int)abs(gyro_rms[2])%1000,
(int)(total_count/3),
(int)(total_count/3),
(int)(total_count/3));
}
