static int read_accel_raw_xyz_cal(s16 *x, s16 *y, s16 *z)
{
s16 raw_x;
s16 raw_y;
s16 raw_z;
if (!(gb_mpu_data->enabled_sensors & MPU6050_SENSOR_ACCEL)) {
mpu6050_input_resume_accel(gb_mpu_data);
usleep_range(10000, 11000);
}
read_accel_raw_xyz(&raw_x, &raw_y, &raw_z);
*x = raw_x - gb_mpu_data->acc_cal[0];
*y = raw_y - gb_mpu_data->acc_cal[1];
*z = raw_z - gb_mpu_data->acc_cal[2];
if (!(gb_mpu_data->enabled_sensors & MPU6050_SENSOR_ACCEL)) {
mpu6050_input_suspend_accel(gb_mpu_data);
usleep_range(10000, 11000);
}
return 0;
}
static int accel_do_calibrate(int enable)
{
struct file *cal_filp;
int sum[3] = { 0, };
int err;
int i;
struct yas_vector raw;
mm_segment_t old_fs;
struct yas_acc_private_data *data = yas_acc_get_data();
mutex_lock(&data->data_mutex);
yas_acc_set_enable_factory_test(data->driver, 1);
mutex_unlock(&data->data_mutex);
for (i = 0; i < 100; i++) {
err = read_accel_raw_xyz(&raw);
if (err < 0) {
pr_err("%s: accel_read_accel_raw_xyz() "
"failed in the %dth loop\n", __func__, i);
return err;
}
sum[0] += raw.v[0];
sum[1] += raw.v[1];
sum[2] += raw.v[2];
}
mutex_lock(&data->data_mutex);
yas_acc_set_enable_factory_test(data->driver, 0);
mutex_unlock(&data->data_mutex);
if (enable) {
data->cal_data.v[0] =
(sum[0] / 100);
data->cal_data.v[1] =
(sum[1] / 100);
data->cal_data.v[2] =
#if (YAS_ACC_DRIVER == YAS_ACC_DRIVER_LIS3DH)
((sum[2] / 100) - HAL_RESOLUTION);
data->calibrate = CAL_SUCCESS;
#else
((sum[2] / 100) - 256);
#endif
} else {
data->cal_data.v[0] = 0;
data->cal_data.v[1] = 0;
data->cal_data.v[2] = 0;
#if (YAS_ACC_DRIVER == YAS_ACC_DRIVER_LIS3DH)
data->calibrate = CAL_FAIL;
#endif
}
pr_info("%s: cal data (%d,%d,%d)\n", __func__,
data->cal_data.v[0], data->cal_data.v[1],
data->cal_data.v[2]);
old_fs = get_fs();
set_fs(KERNEL_DS);
cal_filp = filp_open(data->acc_pdata->cal_path,
O_CREAT | O_TRUNC | O_WRONLY,
S_IRUGO | S_IWUSR | S_IWGRP);
if (IS_ERR(cal_filp)) {
pr_err("%s: Can't open calibration file\n", __func__);
set_fs(old_fs);
err = PTR_ERR(cal_filp);
return err;
}
err = cal_filp->f_op->write(cal_filp,
#if (YAS_ACC_DRIVER == YAS_ACC_DRIVER_LIS3DH)
(char *)&data->cal_data, 3 * sizeof(s32), &cal_filp->f_pos);
if (err != 3 * sizeof(s32)) {
#else
(char *)&data->cal_data, 3 * sizeof(s16), &cal_filp->f_pos);
if (err != 3 * sizeof(s16)) {
#endif
pr_err("%s: Can't write the cal data to file\n", __func__);
err = -EIO;
#if (YAS_ACC_DRIVER == YAS_ACC_DRIVER_LIS3DH)
data->calibrate = CAL_FAIL;
#endif
}
filp_close(cal_filp, current->files);
set_fs(old_fs);
return err;
}
static int accel_do_calibrate(int enable)
{
struct file *cal_filp;
int sum[3] = { 0, };
int err;
int i;
s16 x;
s16 y;
s16 z;
mm_segment_t old_fs;
if (!(gb_mpu_data->enabled_sensors & MPU6050_SENSOR_ACCEL)) {
mpu6050_input_resume_accel(gb_mpu_data);
usleep_range(10000, 11000);
}
for (i = 0; i < ACC_CAL_TIME; i++) {
err = read_accel_raw_xyz(&x, &y, &z);
if (err < 0) {
pr_err("%s: accel_read_accel_raw_xyz() "
"failed in the %dth loop\n", __func__, i);
goto done;
}
usleep_range(10000, 11000);
sum[0] += x/ACC_CAL_DIV;
sum[1] += y/ACC_CAL_DIV;
sum[2] += z/ACC_CAL_DIV;
}
if (!(gb_mpu_data->enabled_sensors & MPU6050_SENSOR_ACCEL))
mpu6050_input_suspend_accel(gb_mpu_data);
if (enable) {
gb_mpu_data->acc_cal[0] =
(sum[0] / ACC_CAL_TIME) * ACC_CAL_DIV;
gb_mpu_data->acc_cal[1] =
(sum[1] / ACC_CAL_TIME) * ACC_CAL_DIV;
gb_mpu_data->acc_cal[2] =
((sum[2] / ACC_CAL_TIME) - ACC_IDEAL) * ACC_CAL_DIV;
} else {
gb_mpu_data->acc_cal[0] = 0;
gb_mpu_data->acc_cal[1] = 0;
gb_mpu_data->acc_cal[2] = 0;
}
pr_info("%s: cal data (%d,%d,%d)\n", __func__,
gb_mpu_data->acc_cal[0], gb_mpu_data->acc_cal[1],
gb_mpu_data->acc_cal[2]);
old_fs = get_fs();
set_fs(KERNEL_DS);
cal_filp = filp_open(gb_mpu_data->pdata->acc_cal_path,
O_CREAT | O_TRUNC | O_WRONLY,
S_IRUGO | S_IWUSR | S_IWGRP);
if (IS_ERR(cal_filp)) {
pr_err("%s: Can't open calibration file\n", __func__);
set_fs(old_fs);
err = PTR_ERR(cal_filp);
goto done;
}
err = cal_filp->f_op->write(cal_filp,
(char *)&gb_mpu_data->acc_cal, 3 * sizeof(s16),
&cal_filp->f_pos);
if (err != 3 * sizeof(s16)) {
pr_err("%s: Can't write the cal data to file\n", __func__);
err = -EIO;
}
filp_close(cal_filp, current->files);
done:
set_fs(old_fs);
return err;
}
