2 * This program is free software; you can redistribute it and/or modify
3 * it under the terms of the GNU General Public License as published by
4 * the Free Software Foundation; either version 2 of the License, or
5 * (at your option) any later version.
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software
14 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
16 * Copyright (c) 2008 Andy Green <andy@openmoko.com>
19 * Median averaging stuff. We sort incoming raw samples into an array of
20 * MEDIAN_SIZE length, discarding the oldest sample each time once we are full.
21 * We then return the sum of the middle three samples for X and Y. It means
22 * the final result must be divided by (3 * scaling factor) to correct for
23 * avoiding the repeated /3.
25 * This strongly rejects brief excursions away from a central point that is
26 * sticky in time compared to the excursion duration.
28 * Thanks to Dale Schumacher (who wrote some example code) and Carl-Daniel
29 * Halifinger who pointed out this would be a good method.
32 #include <linux/errno.h>
33 #include <linux/kernel.h>
34 #include <linux/slab.h>
35 #include <linux/touchscreen/ts_filter_median.h>
37 struct ts_filter_median
{
38 /* Private configuration. */
39 struct ts_filter_median_configuration
*config
;
40 /* Generic Filter API. */
43 /* Count raw samples we get. */
46 * Remember the last coordinates we got in order to know if
47 * we are moving slow or fast.
49 int last_issued
[MAX_TS_FILTER_COORDS
];
50 /* How many samples in the sort buffer are valid. */
52 /* Samples taken for median in sorted form. */
53 int *sort
[MAX_TS_FILTER_COORDS
];
54 /* Samples taken for median. */
55 int *fifo
[MAX_TS_FILTER_COORDS
];
56 /* Where we are in the fifo sample memory. */
58 /* Do we have a sample to deliver? */
62 #define ts_filter_to_filter_median(f) \
63 container_of(f, struct ts_filter_median, tsf)
66 static void ts_filter_median_insert(int *p
, int sample
, int count
)
70 /* Search through what we got so far to find where to put sample. */
71 for (n
= 0; n
< count
; n
++)
72 if (sample
< p
[n
]) { /* We met somebody bigger than us? */
73 /* Starting from the end, push bigger guys down one. */
74 for (count
--; count
>= n
; count
--)
75 p
[count
+ 1] = p
[count
];
76 p
[n
] = sample
; /* Put us in place of first bigger. */
80 p
[count
] = sample
; /* Nobody was bigger than us, add us on the end. */
83 static void ts_filter_median_del(int *p
, int value
, int count
)
87 for (index
= 0; index
< count
; index
++)
88 if (p
[index
] == value
) {
89 for (; index
< count
; index
++)
90 p
[index
] = p
[index
+ 1];
96 static void ts_filter_median_clear(struct ts_filter
*tsf
)
98 struct ts_filter_median
*tsfm
= ts_filter_to_filter_median(tsf
);
103 memset(&tsfm
->last_issued
[0], 1, tsf
->count_coords
* sizeof(int));
106 static struct ts_filter
*ts_filter_median_create(
107 struct platform_device
*pdev
,
108 const struct ts_filter_configuration
*conf
,
113 struct ts_filter_median
*tsfm
= kzalloc(sizeof(struct ts_filter_median
),
119 tsfm
->config
= container_of(conf
,
120 struct ts_filter_median_configuration
,
123 tsfm
->tsf
.count_coords
= count_coords
;
125 tsfm
->config
->midpoint
= (tsfm
->config
->extent
>> 1) + 1;
127 p
= kmalloc(2 * count_coords
* sizeof(int) * (tsfm
->config
->extent
+ 1),
134 for (n
= 0; n
< count_coords
; n
++) {
136 p
+= tsfm
->config
->extent
+ 1;
138 p
+= tsfm
->config
->extent
+ 1;
141 ts_filter_median_clear(&tsfm
->tsf
);
144 "Created Median filter len:%d coords:%d dec_threshold:%d\n",
145 tsfm
->config
->extent
, count_coords
,
146 tsfm
->config
->decimation_threshold
);
151 static void ts_filter_median_destroy(struct ts_filter
*tsf
)
153 struct ts_filter_median
*tsfm
= ts_filter_to_filter_median(tsf
);
155 kfree(tsfm
->sort
[0]); /* First guy has pointer from kmalloc. */
159 static void ts_filter_median_scale(struct ts_filter
*tsf
, int *coords
)
163 for (n
= 0; n
< tsf
->count_coords
; n
++)
164 coords
[n
] = (coords
[n
] + 2) / 3;
168 * Give us the raw sample data coords, and if we return 1 then you can
169 * get a filtered coordinate from coords. If we return 0 you didn't
170 * fill all the filters with samples yet.
173 static int ts_filter_median_process(struct ts_filter
*tsf
, int *coords
)
175 struct ts_filter_median
*tsfm
= ts_filter_to_filter_median(tsf
);
179 for (n
= 0; n
< tsf
->count_coords
; n
++) {
180 /* Grab copy in insertion order to remove when oldest. */
181 tsfm
->fifo
[n
][tsfm
->pos
] = coords
[n
];
182 /* Insert these samples in sorted order in the median arrays. */
183 ts_filter_median_insert(tsfm
->sort
[n
], coords
[n
], tsfm
->valid
);
185 /* Move us on in the fifo. */
186 if (++tsfm
->pos
== (tsfm
->config
->extent
+ 1))
189 /* Have we finished a median sampling? */
190 if (++tsfm
->valid
< tsfm
->config
->extent
)
191 goto process_exit
; /* No valid sample to use. */
193 BUG_ON(tsfm
->valid
!= tsfm
->config
->extent
);
198 * Sum the middle 3 in the median sorted arrays. We don't divide back
199 * down which increases the sum resolution by a factor of 3 until the
200 * scale API function is called.
202 for (n
= 0; n
< tsf
->count_coords
; n
++)
203 /* Perform the deletion of the oldest sample. */
204 ts_filter_median_del(tsfm
->sort
[n
], tsfm
->fifo
[n
][tsfm
->pos
],
207 tsfm
->samples_count
--;
208 if (tsfm
->samples_count
>= 0)
211 for (n
= 0; n
< tsf
->count_coords
; n
++) {
212 /* Give the coordinate result from summing median 3. */
213 coords
[n
] = tsfm
->sort
[n
][tsfm
->config
->midpoint
- 1] +
214 tsfm
->sort
[n
][tsfm
->config
->midpoint
] +
215 tsfm
->sort
[n
][tsfm
->config
->midpoint
+ 1];
217 movement
+= abs(tsfm
->last_issued
[n
] - coords
[n
]);
220 if (movement
> tsfm
->config
->decimation_threshold
) /* Moving fast. */
221 tsfm
->samples_count
= tsfm
->config
->decimation_above
;
223 tsfm
->samples_count
= tsfm
->config
->decimation_below
;
225 memcpy(&tsfm
->last_issued
[0], coords
, tsf
->count_coords
* sizeof(int));
233 static int ts_filter_median_haspoint(struct ts_filter
*tsf
)
235 struct ts_filter_median
*priv
= ts_filter_to_filter_median(tsf
);
240 static void ts_filter_median_getpoint(struct ts_filter
*tsf
, int *point
)
242 struct ts_filter_median
*priv
= ts_filter_to_filter_median(tsf
);
244 BUG_ON(!priv
->ready
);
246 memcpy(point
, &priv
->last_issued
[0], tsf
->count_coords
* sizeof(int));
251 const struct ts_filter_api ts_filter_median_api
= {
252 .create
= ts_filter_median_create
,
253 .destroy
= ts_filter_median_destroy
,
254 .clear
= ts_filter_median_clear
,
255 .process
= ts_filter_median_process
,
256 .scale
= ts_filter_median_scale
,
257 .haspoint
= ts_filter_median_haspoint
,
258 .getpoint
= ts_filter_median_getpoint
,
260 EXPORT_SYMBOL_GPL(ts_filter_median_api
);