From 34f9877cd41fdf8a789f46569cf83109c56a508d Mon Sep 17 00:00:00 2001 From: "lw@rock-chips.com" Date: Sun, 1 Apr 2012 18:16:37 +0800 Subject: [PATCH] rk30:improve gsensors precision --- drivers/input/gsensor/bma023.c | 52 ++++++++++---- drivers/input/gsensor/mma7660.c | 13 ++-- drivers/input/gsensor/mma8452.c | 118 +++++++++++++++++++++++--------- include/linux/mma7660.h | 3 +- include/linux/mma8452.h | 41 ++++++++--- 5 files changed, 164 insertions(+), 63 deletions(-) diff --git a/drivers/input/gsensor/bma023.c b/drivers/input/gsensor/bma023.c index 4fe4f4979d60..3d06c8bb6cc6 100755 --- a/drivers/input/gsensor/bma023.c +++ b/drivers/input/gsensor/bma023.c @@ -186,6 +186,11 @@ #define BMA150_RANGE_4G 1 #define BMA150_RANGE_8G 2 +#define BMA150_RANGE 2000000 +#define BMA150_PRECISION 10 +#define BMA150_BOUNDARY (0x1 << (BMA150_PRECISION - 1)) +#define BMA150_GRAVITY_STEP BMA150_RANGE / BMA150_BOUNDARY + #define BMA150_BW_25HZ 0 #define BMA150_BW_50HZ 1 #define BMA150_BW_100HZ 2 @@ -201,7 +206,7 @@ #define BMA150_MODE_WAKE_UP 3 struct bma150acc{ - s16 x, + s64 x, y, z; } ; @@ -413,28 +418,49 @@ static int bma150_read_accel_xyz(struct i2c_client *client, acc->x = BMA150_GET_BITSLICE(data[0], BMA150_ACC_X_LSB) | (BMA150_GET_BITSLICE(data[1], BMA150_ACC_X_MSB)<< - BMA150_ACC_X_LSB__LEN); + BMA150_ACC_X_LSB__LEN); + if (acc->x < BMA150_BOUNDARY) + acc->x = acc->x * BMA150_GRAVITY_STEP; + else + acc->x = ~( ((~acc->x & (0x7fff>>(16-BMA150_PRECISION)) ) + 1) + * BMA150_GRAVITY_STEP) + 1; +#if 0 acc->x = acc->x << (sizeof(short)*8-(BMA150_ACC_X_LSB__LEN+ BMA150_ACC_X_MSB__LEN)); acc->x = acc->x >> (sizeof(short)*8-(BMA150_ACC_X_LSB__LEN+ BMA150_ACC_X_MSB__LEN)); +#endif acc->y = BMA150_GET_BITSLICE(data[2], BMA150_ACC_Y_LSB) | (BMA150_GET_BITSLICE(data[3], BMA150_ACC_Y_MSB)<< BMA150_ACC_Y_LSB__LEN); + if (acc->y < BMA150_BOUNDARY) + acc->y = acc->y * BMA150_GRAVITY_STEP; + else + acc->y = ~( ((~acc->y & (0x7fff>>(16-BMA150_PRECISION)) ) + 1) + * BMA150_GRAVITY_STEP) + 1; +#if 0 acc->y = acc->y << (sizeof(short)*8-(BMA150_ACC_Y_LSB__LEN + BMA150_ACC_Y_MSB__LEN)); acc->y = acc->y >> (sizeof(short)*8-(BMA150_ACC_Y_LSB__LEN + BMA150_ACC_Y_MSB__LEN)); +#endif acc->z = BMA150_GET_BITSLICE(data[4], BMA150_ACC_Z_LSB); acc->z |= (BMA150_GET_BITSLICE(data[5], BMA150_ACC_Z_MSB)<< BMA150_ACC_Z_LSB__LEN); + if (acc->z < BMA150_BOUNDARY) + acc->z = acc->z * BMA150_GRAVITY_STEP; + else + acc->z = ~( ((~acc->z & (0x7fff>>(16-BMA150_PRECISION)) ) + 1) + * BMA150_GRAVITY_STEP) + 1; +#if 0 acc->z = acc->z << (sizeof(short)*8-(BMA150_ACC_Z_LSB__LEN+ BMA150_ACC_Z_MSB__LEN)); acc->z = acc->z >> (sizeof(short)*8-(BMA150_ACC_Z_LSB__LEN+ BMA150_ACC_Z_MSB__LEN)); +#endif } @@ -446,7 +472,7 @@ static void bma150_work_func(struct work_struct *work) struct bma150_data *bma150 = container_of((struct delayed_work *)work, struct bma150_data, work); static struct bma150acc acc; - s16 x,y,z; + s32 x,y,z; unsigned long delay = msecs_to_jiffies(atomic_read(&bma150->delay)); struct bma023_platform_data *pdata = pdata = (bma150->bma150_client)->dev.platform_data; @@ -461,9 +487,9 @@ static void bma150_work_func(struct work_struct *work) y = acc.y; z = acc.z; } - input_report_abs(bma150->input, ABS_X, x<<2); - input_report_abs(bma150->input, ABS_Y, y<<2); - input_report_abs(bma150->input, ABS_Z, z<<2); + input_report_abs(bma150->input, ABS_X, x); + input_report_abs(bma150->input, ABS_Y, y); + input_report_abs(bma150->input, ABS_Z, z); input_sync(bma150->input); mutex_lock(&bma150->value_mutex); bma150->value.x = x; @@ -582,7 +608,7 @@ static ssize_t bma150_value_show(struct device *dev, acc_value = bma150->value; mutex_unlock(&bma150->value_mutex); - return sprintf(buf, "%d %d %d\n", acc_value.x, acc_value.y, + return sprintf(buf, "%ll %ll %ll\n", acc_value.x, acc_value.y, acc_value.z); } @@ -812,12 +838,12 @@ static long bma023_ioctl( struct file *file, unsigned int cmd, unsigned long arg case BMA_IOCTL_GETDATA: mutex_lock(&bma150->value_mutex); - if(abs(sense_data.x-bma150->value.x)>10)//·À¶¶¶¯ - sense_data.x=bma150->value.x<<2; - if(abs(sense_data.y-(bma150->value.y))>10)//·À¶¶¶¯ - sense_data.y=bma150->value.y<<2; - if(abs(sense_data.z-(bma150->value.z))>10)//·À¶¶¶¯ - sense_data.z=bma150->value.z<<2; + if(abs(sense_data.x-bma150->value.x)>40000)//·À¶¶¶¯ + sense_data.x=bma150->value.x; + if(abs(sense_data.y-(bma150->value.y))>40000)//·À¶¶¶¯ + sense_data.y=bma150->value.y; + if(abs(sense_data.z-(bma150->value.z))>40000)//·À¶¶¶¯ + sense_data.z=bma150->value.z; //bma150->value = acc; mutex_unlock(&bma150->value_mutex); diff --git a/drivers/input/gsensor/mma7660.c b/drivers/input/gsensor/mma7660.c index 585f6eace78b..bf6c734802bd 100644 --- a/drivers/input/gsensor/mma7660.c +++ b/drivers/input/gsensor/mma7660.c @@ -319,8 +319,7 @@ static int mma7660_release(struct inode *inode, struct file *file) return 0; } -static int mma7660_ioctl(struct inode *inode, struct file *file, unsigned int cmd, - unsigned long arg) +static long mma7660_ioctl(struct file *file, unsigned int cmd, unsigned long arg) { void __user *argp = (void __user *)arg; @@ -414,12 +413,12 @@ static struct file_operations mma7660_fops = { .owner = THIS_MODULE, .open = mma7660_open, .release = mma7660_release, - .ioctl = mma7660_ioctl, + .unlocked_ioctl = mma7660_ioctl, }; static struct miscdevice mma7660_device = { .minor = MISC_DYNAMIC_MINOR, - .name = "gsensor",//"mma7660_daemon", + .name = "mma8452_daemon",//"mma7660_daemon", .fops = &mma7660_fops, }; @@ -562,11 +561,11 @@ static int mma7660_probe(struct i2c_client *client, const struct i2c_device_id set_bit(EV_ABS, mma7660->input_dev->evbit); /* x-axis acceleration */ - input_set_abs_params(mma7660->input_dev, ABS_X, -1500, 1500, 0, 0); + input_set_abs_params(mma7660->input_dev, ABS_X, -MMA7660_RANGE, MMA7660_RANGE, 0, 0); /* y-axis acceleration */ - input_set_abs_params(mma7660->input_dev, ABS_Y, -1500, 1500, 0, 0); + input_set_abs_params(mma7660->input_dev, ABS_Y, -MMA7660_RANGE, MMA7660_RANGE, 0, 0); /* z-axis acceleration */ - input_set_abs_params(mma7660->input_dev, ABS_Z, -1500, 1500, 0, 0); + input_set_abs_params(mma7660->input_dev, ABS_Z, -MMA7660_RANGE, MMA7660_RANGE, 0, 0); mma7660->input_dev->name = "compass"; mma7660->input_dev->dev.parent = &client->dev; diff --git a/drivers/input/gsensor/mma8452.c b/drivers/input/gsensor/mma8452.c index b745fb5b5b7b..37f469a4a509 100755 --- a/drivers/input/gsensor/mma8452.c +++ b/drivers/input/gsensor/mma8452.c @@ -56,6 +56,7 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id #define MMA8452_SPEED 200 * 1000 #define MMA8451_DEVID 0x1a #define MMA8452_DEVID 0x2a +#define MMA8453_DEVID 0x3a /* Addresses to scan -- protected by sense_data_mutex */ //static char sense_data[RBUFF_SIZE + 1]; static struct i2c_client *this_client; @@ -66,8 +67,9 @@ static DECLARE_WAIT_QUEUE_HEAD(data_ready_wq); #ifdef CONFIG_HAS_EARLYSUSPEND static struct early_suspend mma8452_early_suspend; #endif -//static int revision = -1; +static int revision = -1; static const char* vendor = "Freescale Semiconductor"; +static char devid; typedef char status_t; @@ -197,7 +199,6 @@ static int mma845x_active(struct i2c_client *client,int enable) return ret; } -#if 0 static int mma8452_start_test(struct i2c_client *client) { int ret = 0; @@ -242,7 +243,6 @@ static int mma8452_start_test(struct i2c_client *client) return ret; } -#endif static int mma8452_start_dev(struct i2c_client *client, char rate) { @@ -328,17 +328,56 @@ static int mma8452_reset_rate(struct i2c_client *client, char rate) return ret ; } -static inline int mma8452_convert_to_int(char value) +static inline int mma8452_convert_to_int(const char high_byte, const char low_byte) { - int result; - - if (value < MMA8452_BOUNDARY) { - result = value * MMA8452_GRAVITY_STEP / 10; + s64 result; +/* enabled only if FREAD MODE */ +/* + if (high_byte < MMA8452_BOUNDARY) { + result = high_byte * MMA8452_GRAVITY_STEP; } else { - result = ~(((~value & 0x7f) + 1)* MMA8452_GRAVITY_STEP / 10) + 1; + result = ~(((~high_byte&0x7f) + 1) * MMA8452_GRAVITY_STEP) + 1; + } +*/ + switch (devid) { + + case MMA8451_DEVID: + result = ((int)high_byte << (MMA8451_PRECISION-8)) + | ((int)low_byte >> (16-MMA8451_PRECISION)); + if (result < MMA8451_BOUNDARY) + result = result* MMA8451_GRAVITY_STEP; + else + result = ~( ((~result & (0x7fff>>(16-MMA8451_PRECISION)) ) + 1) + * MMA8451_GRAVITY_STEP) + 1; + break; + + case MMA8452_DEVID: + result = ((int)high_byte << (MMA8452_PRECISION-8)) + | ((int)low_byte >> (16-MMA8452_PRECISION)); + if (result < MMA8452_BOUNDARY) + result = result* MMA8452_GRAVITY_STEP; + else + result = ~( ((~result & (0x7fff>>(16-MMA8452_PRECISION)) ) + 1) + * MMA8452_GRAVITY_STEP) + 1; + break; + + case MMA8453_DEVID: + result = ((int)high_byte << (MMA8453_PRECISION-8)) + | ((int)low_byte >> (16-MMA8453_PRECISION)); + if (result < MMA8453_BOUNDARY) + result = result* MMA8453_GRAVITY_STEP; + else + result = ~( ((~result & (0x7fff>>(16-MMA8453_PRECISION)) ) + 1) + * MMA8453_GRAVITY_STEP) + 1; + break; + + default: + mmaprintk(KERN_ERR + "mma8452_convert_to_int: devid wasn't set correctly\n"); + return -EFAULT; } - return result; + return (int)result; } static void mma8452_report_value(struct i2c_client *client, struct mma8452_axis *axis) @@ -358,26 +397,38 @@ static void mma8452_report_value(struct i2c_client *client, struct mma8452_axis static int mma8452_get_data(struct i2c_client *client) { struct mma8452_data* mma8452 = i2c_get_clientdata(client); - char buffer[6]; int ret; int x,y,z; struct mma8452_axis axis; struct mma8452_platform_data *pdata = pdata = client->dev.platform_data; +/* enabled only if FREAD MODE */ +/* + char buffer[3]; do { memset(buffer, 0, 3); buffer[0] = MMA8452_REG_X_OUT_MSB; - //ret = mma8452_tx_data(client, &buffer[0], 1); ret = mma8452_rx_data(client, &buffer[0], 3); if (ret < 0) return ret; } while (0); - mmaprintkd("0x%02x 0x%02x 0x%02x \n",buffer[0],buffer[1],buffer[2]); - - x = mma8452_convert_to_int(buffer[0]); - y = mma8452_convert_to_int(buffer[1]); - z = mma8452_convert_to_int(buffer[2]); + x = mma8452_convert_to_int(buffer[0],0); + y = mma8452_convert_to_int(buffer[1],0); + z = mma8452_convert_to_int(buffer[2],0); +*/ + char buffer[6]; + do { + memset(buffer, 0, 6); + buffer[0] = MMA8452_REG_X_OUT_MSB; + ret = mma8452_rx_data(client, &buffer[0], 6); + if (ret < 0) + return ret; + } while (0); + + x = mma8452_convert_to_int(buffer[0],buffer[1]); + y = mma8452_convert_to_int(buffer[2],buffer[3]); + z = mma8452_convert_to_int(buffer[4],buffer[5]); if (pdata->swap_xyz) { axis.x = (pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z; @@ -629,9 +680,9 @@ static void mma8452_resume(struct early_suspend *h) #else static int mma8452_suspend(struct i2c_client *client, pm_message_t mesg) { - int ret = 0; - //struct mma8452_data *mma8452 = (struct mma8452_data *)i2c_get_clientdata(client); + int ret; mmaprintkd("Gsensor mma7760 enter 2 level suspend mma8452->status %d\n",mma8452->status); + struct mma8452_data *mma8452 = (struct mma8452_data *)i2c_get_clientdata(client); // if(mma8452->status == MMA8452_OPEN) // { // mma8452->status = MMA8452_SUSPEND; @@ -641,8 +692,8 @@ static int mma8452_suspend(struct i2c_client *client, pm_message_t mesg) } static int mma8452_resume(struct i2c_client *client) { - int ret = 0; - //struct mma8452_data *mma8452 = (struct mma8452_data *)i2c_get_clientdata(client); + int ret; + struct mma8452_data *mma8452 = (struct mma8452_data *)i2c_get_clientdata(client); mmaprintkd("Gsensor mma7760 2 level resume!! mma8452->status %d\n",mma8452->status); // if((mma8452->status == MMA8452_SUSPEND) && (mma8452->status != MMA8452_OPEN)) //if (mma8452->status == MMA8452_OPEN) @@ -712,7 +763,6 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id struct mma8452_data *mma8452; struct mma8452_platform_data *pdata = pdata = client->dev.platform_data; int err; - char devid; mmaprintkf("%s enter\n",__FUNCTION__); @@ -743,7 +793,9 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id this_client = client; devid = mma8452_get_devid(this_client); - if ((MMA8452_DEVID != devid) && (MMA8451_DEVID != devid)) { + if ((MMA8452_DEVID != devid) + && (MMA8451_DEVID != devid) + && (MMA8453_DEVID != devid)) { pr_info("mma8452: invalid devid\n"); goto exit_invalid_devid; } @@ -766,11 +818,11 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id set_bit(EV_ABS, mma8452->input_dev->evbit); /* x-axis acceleration */ - input_set_abs_params(mma8452->input_dev, ABS_X, -2000, 2000, 0, 0); //2g full scale range + input_set_abs_params(mma8452->input_dev, ABS_X, -MMA845X_RANGE, MMA845X_RANGE, 0, 0); //2g full scale range /* y-axis acceleration */ - input_set_abs_params(mma8452->input_dev, ABS_Y, -2000, 2000, 0, 0); //2g full scale range + input_set_abs_params(mma8452->input_dev, ABS_Y, -MMA845X_RANGE, MMA845X_RANGE, 0, 0); //2g full scale range /* z-axis acceleration */ - input_set_abs_params(mma8452->input_dev, ABS_Z, -2000, 2000, 0, 0); //2g full scale range + input_set_abs_params(mma8452->input_dev, ABS_Z, -MMA845X_RANGE, MMA845X_RANGE, 0, 0); //2g full scale range // mma8452->input_dev->name = "compass"; mma8452->input_dev->name = "gsensor"; @@ -784,7 +836,7 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id goto exit_input_register_device_failed; } - mma8452_device.parent = &client->dev; + mma8452_device.parent = &client->dev; err = misc_register(&mma8452_device); if (err < 0) { mmaprintk(KERN_ERR @@ -800,10 +852,10 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id } #ifdef CONFIG_HAS_EARLYSUSPEND - mma8452_early_suspend.suspend = mma8452_suspend; - mma8452_early_suspend.resume = mma8452_resume; - mma8452_early_suspend.level = 0x2; - register_early_suspend(&mma8452_early_suspend); + mma8452_early_suspend.suspend = mma8452_suspend; + mma8452_early_suspend.resume = mma8452_resume; + mma8452_early_suspend.level = 0x2; + register_early_suspend(&mma8452_early_suspend); #endif printk(KERN_INFO "mma8452 probe ok\n"); @@ -814,9 +866,9 @@ static int mma8452_probe(struct i2c_client *client, const struct i2c_device_id return 0; exit_gsensor_sysfs_init_failed: - misc_deregister(&mma8452_device); + misc_deregister(&mma8452_device); exit_misc_device_register_mma8452_device_failed: - input_unregister_device(mma8452->input_dev); + input_unregister_device(mma8452->input_dev); exit_input_register_device_failed: input_free_device(mma8452->input_dev); exit_input_allocate_device_failed: diff --git a/include/linux/mma7660.h b/include/linux/mma7660.h index 8df0414485e9..ea2f68175717 100644 --- a/include/linux/mma7660.h +++ b/include/linux/mma7660.h @@ -57,9 +57,10 @@ #define MMA7660_IIC_ADDR 0x98 #define MMA7660_REG_LEN 11 -#define MMA7660_GRAVITY_STEP 47 +#define MMA7660_RANGE 2000000 #define MMA7660_PRECISION 6 #define MMA7660_BOUNDARY (0x1 << (MMA7660_PRECISION - 1)) +#define MMA7660_GRAVITY_STEP MMA7660_RANGE/MMA7660_BOUNDARY #define MMA7660_TOTAL_TIME 10 diff --git a/include/linux/mma8452.h b/include/linux/mma8452.h index 9de60414684a..6bc81c2a5b0e 100755 --- a/include/linux/mma8452.h +++ b/include/linux/mma8452.h @@ -85,23 +85,46 @@ #define MMA8452_ASLP_RATE_1P56 3 #define MMA8452_ASLP_RATE_SHIFT 6 -#define ACTIVE_MASK 1 -#define FREAD_MASK 2 +/*Auto-adapt mma845x series*/ +/*Modified by Yick @ROCKCHIP + xieyi@rockchips.com*/ +/* + Range: unit(ug 1g=1 000 000 ug) + option(2g,4g,8g) + G would be defined on android HAL + Precision: bit wide of valid data + Boundary: Max positive count + Gravity_step: gravity value indicated by per count + */ +#define FREAD_MASK 0 /* enabled(1<<1) only if reading MSB 8bits*/ +#define MMA845X_RANGE 2000000 +/* mma8451 */ +#define MMA8451_PRECISION 14 +#define MMA8451_BOUNDARY (0x1 << (MMA8451_PRECISION - 1)) +#define MMA8451_GRAVITY_STEP MMA845X_RANGE / MMA8451_BOUNDARY + +/* mma8452 */ +#define MMA8452_PRECISION 12 +#define MMA8452_BOUNDARY (0x1 << (MMA8452_PRECISION - 1)) +#define MMA8452_GRAVITY_STEP MMA845X_RANGE / MMA8452_BOUNDARY +/* mma8453 */ +#define MMA8453_PRECISION 10 +#define MMA8453_BOUNDARY (0x1 << (MMA8453_PRECISION - 1)) +#define MMA8453_GRAVITY_STEP MMA845X_RANGE / MMA8453_BOUNDARY + +/*End of precision adaption*/ + +#define MMA8452_TOTAL_TIME 10 + +#define ACTIVE_MASK 1 /*status*/ #define MMA8452_SUSPEND 2 #define MMA8452_OPEN 1 #define MMA8452_CLOSE 0 - - -//#define MMA8452_IIC_ADDR 0x98 #define MMA8452_REG_LEN 11 -#define MMA8452_GRAVITY_STEP 156 //2g full scale range -#define MMA8452_PRECISION 8 //8bit data -#define MMA8452_BOUNDARY (0x1 << (MMA8452_PRECISION - 1)) -#define MMA8452_TOTAL_TIME 10 /* -- 2.34.1