rk30:add data filter for gyroscope

diff --git a/arch/arm/mach-rk30/board-rk30-sdk.c b/arch/arm/mach-rk30/board-rk30-sdk.c
old mode 100644 (file)
new mode 100755 (executable)
index dcbbc91..91fcac4
--- 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 99c003805932e93962e3312c387ebda27343b8ac..e17669b6a3e8146f826410ea7a2956daba32ddb1 100755 (executable)
--- 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 a25277a762d45dab15bc00edfdd160685cb7fdfa..03a5ff018665256d34f9cd1a5d4c3707641a492c 100755 (executable)
--- 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"