From 2593ec204b1ba636d48c66fefd2ef51554e2442a Mon Sep 17 00:00:00 2001 From: lw Date: Fri, 27 Apr 2012 10:04:19 +0800 Subject: [PATCH] rk30:add data filter for gyroscope --- arch/arm/mach-rk30/board-rk30-sdk.c | 4 +- drivers/input/gyroscope/l3g4200d.c | 72 +++++++++++++++++++++-------- include/linux/l3g4200d.h | 40 ++++++---------- 3 files changed, 69 insertions(+), 47 deletions(-) mode change 100644 => 100755 arch/arm/mach-rk30/board-rk30-sdk.c diff --git a/arch/arm/mach-rk30/board-rk30-sdk.c b/arch/arm/mach-rk30/board-rk30-sdk.c old mode 100644 new mode 100755 index dcbbc913213a..91fcac4d93d1 --- a/arch/arm/mach-rk30/board-rk30-sdk.c +++ b/arch/arm/mach-rk30/board-rk30-sdk.c @@ -718,8 +718,10 @@ static int l3g4200d_init_platform_hw(void) static struct l3g4200d_platform_data l3g4200d_info = { .orientation = {0, 1, 0, -1, 0, 0, 0, 0, 1}, - .init = l3g4200d_init_platform_hw, + .x_min = 40,//x_min,y_min,z_min = (0-100) according to hardware + .y_min = 40, + .z_min = 20, }; #endif diff --git a/drivers/input/gyroscope/l3g4200d.c b/drivers/input/gyroscope/l3g4200d.c index 99c003805932..e17669b6a3e8 100755 --- a/drivers/input/gyroscope/l3g4200d.c +++ b/drivers/input/gyroscope/l3g4200d.c @@ -260,15 +260,25 @@ static int l3g4200d_write_reg(struct i2c_client *client,int addr,int value) static char l3g4200d_get_devid(struct i2c_client *client) { - int tempvalue; - tempvalue=l3g4200d_read_reg(client, WHO_AM_I); - if ((tempvalue & 0x00FF) == 0x00D3) { - DBG(KERN_INFO "I2C driver registered!\n"); - return 1; - } else { - DBG(KERN_INFO "I2C driver %d!\n",tempvalue); + unsigned int devid = 0; + struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); + + devid = l3g4200d_read_reg(client, WHO_AM_I)&0xff; + if (devid == GYRO_DEVID_L3G4200D) { + l3g4200d->devid = devid; + printk(KERN_INFO "gyro is L3G4200D and devid=0x%x\n",devid); + return 0; + } else if (devid == GYRO_DEVID_L3G20D) + { + l3g4200d->devid = devid; + printk(KERN_INFO "gyro is L3G20D and devid=0x%x\n",devid); return 0; - } + } + else + { + printk(KERN_ERR "%s:gyro device id is error,devid=%d\n",__func__,devid); + return -1; + } } static int l3g4200d_active(struct i2c_client *client,int enable) @@ -404,8 +414,8 @@ static int l3g4200d_get_data(struct i2c_client *client) //char buffer[6]; int ret,i; struct l3g4200d_axis axis; - struct l3g4200d_platform_data *pdata = pdata = client->dev.platform_data; - + struct l3g4200d_platform_data *pdata = client->dev.platform_data; + struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); //int res; unsigned char gyro_data[6]; /* x,y,z hardware data */ @@ -435,9 +445,20 @@ static int l3g4200d_get_data(struct i2c_client *client) axis.y = y; axis.z = z; } - - l3g4200d_report_value(client, &axis); + //filter gyro data + if((abs(l3g4200d->axis.x - axis.x) > pdata->x_min)||(abs(l3g4200d->axis.y - axis.y) > pdata->y_min)||(abs(l3g4200d->axis.z - axis.z) > pdata->z_min)) + { + l3g4200d->axis.x = axis.x; + l3g4200d->axis.y = axis.y; + l3g4200d->axis.z = axis.z; + if(abs(l3g4200d->axis.x) <= pdata->x_min) l3g4200d->axis.x = 0; + if(abs(l3g4200d->axis.y) <= pdata->y_min) l3g4200d->axis.y = 0; + if(abs(l3g4200d->axis.z) <= pdata->z_min) l3g4200d->axis.z = 0; + + l3g4200d_report_value(client, &l3g4200d->axis); + } + return 0; } @@ -563,7 +584,7 @@ static irqreturn_t l3g4200d_interrupt(int irq, void *dev_id) struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)dev_id; disable_irq_nosync(irq); - schedule_delayed_work(&l3g4200d->delaywork, msecs_to_jiffies(20)); + schedule_delayed_work(&l3g4200d->delaywork, msecs_to_jiffies(10)); DBG("%s :enter\n",__FUNCTION__); return IRQ_HANDLED; } @@ -731,7 +752,7 @@ static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id { struct l3g4200d_data *l3g4200d; struct l3g4200d_platform_data *pdata = pdata = client->dev.platform_data; - int err; + int i,err; if(pdata && pdata->init) pdata->init(); @@ -772,6 +793,19 @@ static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id goto exit_kfree_pdata; } this_data=l3g4200d; + + //try three times + for(i=0; i<3; i++) + { + err = l3g4200d_get_devid(client); + if (!err) + break; + } + if(err) + { + printk("%s:fail\n",__func__); + goto exit_kfree_pdata; + } l3g4200d->input_dev = input_allocate_device(); if (!l3g4200d->input_dev) { @@ -795,7 +829,10 @@ static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id l3g4200d->input_dev->name = "gyro"; l3g4200d->input_dev->dev.parent = &client->dev; - + l3g4200d->axis.x = 0; + l3g4200d->axis.y = 0; + l3g4200d->axis.z = 0; + err = input_register_device(l3g4200d->input_dev); if (err < 0) { DBG(KERN_ERR @@ -825,11 +862,6 @@ static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id l3g4200d_early_suspend.level = 0x2; register_early_suspend(&l3g4200d_early_suspend); #endif - if(l3g4200d_get_devid(this_client)) - printk(KERN_INFO "l3g4200d probe ok\n"); - else - printk(KERN_INFO "l3g4200d probe error\n"); - l3g4200d->status = L3G4200D_CLOSE; #if 0 diff --git a/include/linux/l3g4200d.h b/include/linux/l3g4200d.h index a25277a762d4..03a5ff018665 100755 --- a/include/linux/l3g4200d.h +++ b/include/linux/l3g4200d.h @@ -50,7 +50,6 @@ #define L3G4200D_IOCTL_SET_ENABLE _IOW(L3G4200D_IOCTL_BASE, 2, int) #define L3G4200D_IOCTL_GET_ENABLE _IOR(L3G4200D_IOCTL_BASE, 3, int) - #define L3G4200D_FS_250DPS 0x00 #define L3G4200D_FS_500DPS 0x10 #define L3G4200D_FS_2000DPS 0x30 @@ -79,6 +78,10 @@ #define L3G4200D_REG_CTRL_REG1 0x20 #define ACTIVE_MASK 0x08 +#define GYRO_DEVID_L3G4200D 0xD3 +#define GYRO_DEVID_L3G20D 0xD4 + + #ifdef __KERNEL__ struct l3g4200d_platform_data { u8 fs_range; @@ -91,7 +94,9 @@ struct l3g4200d_platform_data { u8 negate_y; u8 negate_z; signed char orientation[9]; - + int x_min; + int y_min; + int z_min; int (*init)(void); void (*exit)(void); int (*power_on)(void); @@ -101,40 +106,23 @@ struct l3g4200d_platform_data { #endif /* __KERNEL__ */ -#define MMAIO 0xA1 - -/* IOCTLs for MMA8452 library */ -#define ECS_IOCTL_INIT _IO(MMAIO, 0x01) -#define ECS_IOCTL_RESET _IO(MMAIO, 0x04) -#define ECS_IOCTL_CLOSE _IO(MMAIO, 0x02) -#define ECS_IOCTL_START _IO(MMAIO, 0x03) -#define ECS_IOCTL_GETDATA _IOR(MMAIO, 0x08, char[RBUFF_SIZE+1]) - -#define GYROSENSOR_IOCTL_MAGIC 'l' -#define GYROSENSOR_IOCTL_GET_ENABLED _IOR(GYROSENSOR_IOCTL_MAGIC, 1, int *) -#define GYROSENSOR_IOCTL_ENABLE _IOW(GYROSENSOR_IOCTL_MAGIC, 2, int *) - -/* IOCTLs for APPs */ -#define ECS_IOCTL_APP_SET_RATE _IOW(MMAIO, 0x10, char) - -#define EVENT_TYPE_GYRO_X ABS_RY -#define ECS_IOCTL_APP_GET_ABS EVIOCGABS(EVENT_TYPE_GYRO_X) +struct l3g4200d_axis { + int x; + int y; + int z; +}; struct l3g4200d_data { char status; char curr_tate; + unsigned int devid; struct input_dev *input_dev; struct i2c_client *client; struct work_struct work; struct delayed_work delaywork; /*report second event*/ struct l3g4200d_platform_data *pdata; -}; - -struct l3g4200d_axis { - int x; - int y; - int z; + struct l3g4200d_axis axis; }; #define GSENSOR_DEV_PATH "/dev/gyrosensors" -- 2.34.1