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 "ts_filter_median.h"
37 static void ts_filter_median_insert(int *p
, int sample
, int count
)
41 /* search through what we got so far to find where to put sample */
42 for (n
= 0; n
< count
; n
++)
43 /* we met somebody bigger than us? */
45 /* starting from the end, push bigger guys down one */
46 for (count
--; count
>= n
; count
--)
47 p
[count
+ 1] = p
[count
];
48 p
[n
] = sample
; /* and put us in place of first bigger */
52 p
[count
] = sample
; /* nobody was bigger than us, add us on the end */
55 static void ts_filter_median_del(int *p
, int value
, int count
)
59 for (index
= 0; index
< count
; index
++)
60 if (p
[index
] == value
) {
61 for (; index
< count
; index
++)
62 p
[index
] = p
[index
+ 1];
68 static void ts_filter_median_clear_internal(struct ts_filter
*tsf
)
70 struct ts_filter_median
*tsfm
= (struct ts_filter_median
*)tsf
;
76 static void ts_filter_median_clear(struct ts_filter
*tsf
)
78 ts_filter_median_clear_internal(tsf
);
80 if (tsf
->next
) /* chain */
81 (tsf
->next
->api
->clear
)(tsf
->next
);
84 static struct ts_filter
*ts_filter_median_create(struct platform_device
*pdev
,
85 void *conf
, int count_coords
)
89 struct ts_filter_median
*tsfm
= kzalloc(sizeof(struct ts_filter_median
),
95 tsfm
->config
= (struct ts_filter_median_configuration
*)conf
;
96 BUG_ON((count_coords
< 1) || (count_coords
> MAX_TS_FILTER_COORDS
));
97 tsfm
->tsf
.count_coords
= count_coords
;
99 tsfm
->config
->midpoint
= (tsfm
->config
->extent
>> 1) + 1;
101 p
= kmalloc(2 * count_coords
* sizeof(int) * (tsfm
->config
->extent
+ 1),
108 for (n
= 0; n
< count_coords
; n
++) {
110 p
+= tsfm
->config
->extent
+ 1;
112 p
+= tsfm
->config
->extent
+ 1;
115 ts_filter_median_clear_internal(&tsfm
->tsf
);
117 printk(KERN_INFO
" Created Median ts filter len %d depth %d dec %d\n",
118 tsfm
->config
->extent
, count_coords
,
119 tsfm
->config
->decimation_threshold
);
124 static void ts_filter_median_destroy(struct platform_device
*pdev
,
125 struct ts_filter
*tsf
)
127 struct ts_filter_median
*tsfm
= (struct ts_filter_median
*)tsf
;
129 kfree(tsfm
->sort
[0]); /* first guy has pointer from kmalloc */
133 static void ts_filter_median_scale(struct ts_filter
*tsf
, int *coords
)
137 for (n
= 0; n
< tsf
->count_coords
; n
++)
138 coords
[n
] = (coords
[n
] + 2) / 3;
140 if (tsf
->next
) /* chain */
141 (tsf
->next
->api
->scale
)(tsf
->next
, coords
);
145 * Give us the raw sample data coords, and if we return 1 then you can
146 * get a filtered coordinate from coords. If we return 0 you didn't
147 * fill all the filters with samples yet.
150 static int ts_filter_median_process(struct ts_filter
*tsf
, int *coords
)
152 struct ts_filter_median
*tsfm
= (struct ts_filter_median
*)tsf
;
156 for (n
= 0; n
< tsf
->count_coords
; n
++) {
157 /* grab copy in insertion order to remove when oldest */
158 tsfm
->fifo
[n
][tsfm
->pos
] = coords
[n
];
159 /* insert these samples in sorted order in the median arrays */
160 ts_filter_median_insert(tsfm
->sort
[n
], coords
[n
], tsfm
->valid
);
162 /* move us on in the fifo */
163 if (++tsfm
->pos
== (tsfm
->config
->extent
+ 1))
166 /* we have finished a median sampling? */
167 if (++tsfm
->valid
!= tsfm
->config
->extent
)
168 return 0; /* no valid sample to use */
170 /* discard the oldest sample in median sorted array */
174 * Sum the middle 3 in the median sorted arrays. We don't divide back
175 * down which increases the sum resolution by a factor of 3 until the
176 * scale API is called.
178 for (n
= 0; n
< tsfm
->tsf
.count_coords
; n
++)
179 /* perform the deletion of the oldest sample */
180 ts_filter_median_del(tsfm
->sort
[n
], tsfm
->fifo
[n
][tsfm
->pos
],
183 tsfm
->decimation_count
--;
184 if (tsfm
->decimation_count
>= 0)
187 for (n
= 0; n
< tsfm
->tsf
.count_coords
; n
++) {
188 /* give the coordinate result from summing median 3 */
189 coords
[n
] = tsfm
->sort
[n
][tsfm
->config
->midpoint
- 1] +
190 tsfm
->sort
[n
][tsfm
->config
->midpoint
] +
191 tsfm
->sort
[n
][tsfm
->config
->midpoint
+ 1]
194 movement
+= abs(tsfm
->last_issued
[n
] - coords
[n
]);
197 if (movement
> tsfm
->config
->decimation_threshold
) /* fast */
198 tsfm
->decimation_count
= tsfm
->config
->decimation_above
;
200 tsfm
->decimation_count
= tsfm
->config
->decimation_below
;
202 memcpy(&tsfm
->last_issued
[0], coords
,
203 tsfm
->tsf
.count_coords
* sizeof(int));
205 if (tsf
->next
) /* chain */
206 return (tsf
->next
->api
->process
)(tsf
->next
, coords
);
211 struct ts_filter_api ts_filter_median_api
= {
212 .create
= ts_filter_median_create
,
213 .destroy
= ts_filter_median_destroy
,
214 .clear
= ts_filter_median_clear
,
215 .process
= ts_filter_median_process
,
216 .scale
= ts_filter_median_scale
,