1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
|
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
* Copyright (c) 2008 Andy Green <andy@openmoko.com>
*
*
* Median averaging stuff. We sort incoming raw samples into an array of
* MEDIAN_SIZE length, discarding the oldest sample each time once we are full.
* We then return the sum of the middle three samples for X and Y. It means
* the final result must be divided by (3 * scaling factor) to correct for
* avoiding the repeated /3.
*
* This strongly rejects brief excursions away from a central point that is
* sticky in time compared to the excursion duration.
*
* Thanks to Dale Schumacher (who wrote some example code) and Carl-Daniel
* Halifinger who pointed out this would be a good method.
*/
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/ts_filter_median.h>
static void ts_filter_median_insert(int *p, int sample, int count)
{
int n;
/* search through what we got so far to find where to put sample */
for (n = 0; n < count; n++)
/* we met somebody bigger than us? */
if (sample < p[n]) {
/* starting from the end, push bigger guys down one */
for (count--; count >= n; count--)
p[count + 1] = p[count];
p[n] = sample; /* and put us in place of first bigger */
return;
}
p[count] = sample; /* nobody was bigger than us, add us on the end */
}
static void ts_filter_median_del(int *p, int value, int count)
{
int index;
for (index = 0; index < count; index++)
if (p[index] == value) {
for (; index < count; index++)
p[index] = p[index + 1];
return;
}
}
static void ts_filter_median_clear(struct ts_filter *tsf)
{
struct ts_filter_median *tsfm = (struct ts_filter_median *)tsf;
tsfm->pos = 0;
tsfm->valid = 0;
if (tsf->next) /* chain */
(tsf->next->api->clear)(tsf->next);
}
static struct ts_filter *ts_filter_median_create(void * conf, int count_coords)
{
int *p;
int n;
struct ts_filter_median *tsfm = kzalloc(sizeof(struct ts_filter_median),
GFP_KERNEL);
if (!tsfm)
return NULL;
tsfm->config = (struct ts_filter_median_configuration *)conf;
BUG_ON((count_coords < 1) || (count_coords > MAX_TS_FILTER_COORDS));
tsfm->tsf.count_coords = count_coords;
tsfm->config->midpoint = (tsfm->config->extent >> 1) + 1;
printk(KERN_INFO" Creating Median ts filter len %d depth %d dec %d\n",
tsfm->config->extent, count_coords,
tsfm->config->decimation_threshold);
p = kmalloc(2 * count_coords * sizeof(int) * (tsfm->config->extent + 1),
GFP_KERNEL);
if (!p) {
kfree(tsfm);
return NULL;
}
for (n = 0; n < count_coords; n++) {
tsfm->sort[n] = p;
p += tsfm->config->extent + 1;
tsfm->fifo[n] = p;
p += tsfm->config->extent + 1;
}
ts_filter_median_clear(&tsfm->tsf);
return &tsfm->tsf;
}
static void ts_filter_median_destroy(struct ts_filter *tsf)
{
struct ts_filter_median *tsfm = (struct ts_filter_median *)tsf;
kfree(tsfm->sort[0]); /* first guy has pointer from kmalloc */
kfree(tsf);
}
static void ts_filter_median_scale(struct ts_filter *tsf, int *coords)
{
int n;
for (n = 0; n < tsf->count_coords; n++)
coords[n] = (coords[n] + 2) / 3;
if (tsf->next) /* chain */
(tsf->next->api->scale)(tsf->next, coords);
}
/* give us the raw sample data coords, and if we return 1 then you can
* get a filtered coordinate from coords: if we return 0 you didn't
* fill all the filters with samples yet.
*/
static int ts_filter_median_process(struct ts_filter *tsf, int *coords)
{
struct ts_filter_median *tsfm = (struct ts_filter_median *)tsf;
int n;
int movement = 1;
for (n = 0; n < tsf->count_coords; n++) {
/* grab copy in insertion order to remove when oldest */
tsfm->fifo[n][tsfm->pos] = coords[n];
/* insert these samples in sorted order in the median arrays */
ts_filter_median_insert(tsfm->sort[n], coords[n], tsfm->valid);
}
/* move us on in the fifo */
if (++tsfm->pos == (tsfm->config->extent + 1))
tsfm->pos = 0;
/* we have finished a median sampling? */
if (++tsfm->valid != tsfm->config->extent)
return 0; /* no valid sample to use */
/* discard the oldest sample in median sorted array */
tsfm->valid--;
/* sum the middle 3 in the median sorted arrays. We don't divide back
* down which increases the sum resolution by a factor of 3 until the
* scale API is called
*/
for (n = 0; n < tsfm->tsf.count_coords; n++)
/* perform the deletion of the oldest sample */
ts_filter_median_del(tsfm->sort[n], tsfm->fifo[n][tsfm->pos],
tsfm->valid);
tsfm->decimation_count--;
if (tsfm->decimation_count >= 0)
return 0;
for (n = 0; n < tsfm->tsf.count_coords; n++) {
/* give the coordinate result from summing median 3 */
coords[n] = tsfm->sort[n][tsfm->config->midpoint - 1] +
tsfm->sort[n][tsfm->config->midpoint] +
tsfm->sort[n][tsfm->config->midpoint + 1]
;
movement += abs(tsfm->last_issued[n] - coords[n]);
}
if (movement > tsfm->config->decimation_threshold) /* fast */
tsfm->decimation_count = tsfm->config->decimation_above;
else
tsfm->decimation_count = tsfm->config->decimation_below;
memcpy(&tsfm->last_issued, coords, tsfm->tsf.count_coords);
if (tsf->next) /* chain */
return (tsf->next->api->process)(tsf->next, coords);
else
return 1;
}
struct ts_filter_api ts_filter_median_api = {
.create = ts_filter_median_create,
.destroy = ts_filter_median_destroy,
.clear = ts_filter_median_clear,
.process = ts_filter_median_process,
.scale = ts_filter_median_scale,
};
|
