1 // Copyright 2012 The ChromiumOS Authors
2 // Use of this source code is governed by a BSD-style license that can be
3 // found in the LICENSE file.
4
5 #include "include/non_linearity_filter_interpreter.h"
6
7 #include <linux/in.h>
8
9 namespace {
10 const size_t kIntPackedSize = 4;
11 const size_t kDoublePackedSize = 8;
12 }
13
14 namespace gestures {
15
NonLinearityFilterInterpreter(PropRegistry * prop_reg,Interpreter * next,Tracer * tracer)16 NonLinearityFilterInterpreter::NonLinearityFilterInterpreter(
17 PropRegistry* prop_reg,
18 Interpreter* next,
19 Tracer* tracer)
20 : FilterInterpreter(NULL, next, tracer, false),
21 x_range_len_(0), y_range_len_(0), p_range_len_(0),
22 enabled_(prop_reg, "Enable non-linearity correction", false),
23 data_location_(prop_reg, "Non-linearity correction data file", "None") {
24 InitName();
25 LoadData();
26 }
27
ErrorIndex(size_t x_index,size_t y_index,size_t p_index) const28 unsigned int NonLinearityFilterInterpreter::ErrorIndex(size_t x_index,
29 size_t y_index,
30 size_t p_index) const {
31 unsigned int index = x_index * y_range_len_ * p_range_len_ +
32 y_index * p_range_len_ + p_index;
33
34 if (index >= x_range_len_ * y_range_len_ * p_range_len_)
35 index = 0;
36 return index;
37 }
38
ReadObject(void * buf,size_t object_size,FILE * fd)39 int NonLinearityFilterInterpreter::ReadObject(void* buf, size_t object_size,
40 FILE* fd) {
41 int objs_read = fread(buf, object_size, 1, fd);
42 /* If this machine is big endian, reverse the bytes in the object */
43 if (object_size == kDoublePackedSize)
44 __le64_to_cpu(*static_cast<double*>(buf));
45 if (object_size == kIntPackedSize)
46 __le32_to_cpu(*static_cast<int32_t*>(buf));
47
48 return objs_read;
49 }
50
LoadRange(std::unique_ptr<double[]> & arr,size_t & len,FILE * fd)51 bool NonLinearityFilterInterpreter::LoadRange(std::unique_ptr<double[]>& arr,
52 size_t& len, FILE* fd) {
53 int tmp;
54 if (!ReadObject(&tmp, kIntPackedSize, fd))
55 return false;
56 len = tmp;
57
58 arr.reset(new double[len]);
59 for (size_t i = 0; i < len; i++) {
60 double tmp;
61 if (!ReadObject(&tmp, kDoublePackedSize, fd))
62 return false;
63 else
64 arr[i] = tmp;
65 }
66 return true;
67 }
68
LoadData()69 void NonLinearityFilterInterpreter::LoadData() {
70 FILE* data_fd = fopen(data_location_.val_, "rb");
71 if (!data_fd) {
72 Log("Unable to open non-linearity filter data '%s'", data_location_.val_);
73 return;
74 }
75
76 // Load the ranges
77 if (!LoadRange(x_range_, x_range_len_, data_fd))
78 goto abort_load;
79 if (!LoadRange(y_range_, y_range_len_, data_fd))
80 goto abort_load;
81 if (!LoadRange(p_range_, p_range_len_, data_fd))
82 goto abort_load;
83
84 // Load the error readings themselves
85 err_.reset(new Error[x_range_len_ * y_range_len_ * p_range_len_]);
86 Error tmp;
87 for(unsigned int x = 0; x < x_range_len_; x++) {
88 for(unsigned int y = 0; y < y_range_len_; y++) {
89 for(unsigned int p = 0; p < p_range_len_; p++) {
90 if (!ReadObject(&tmp.x_error, kDoublePackedSize, data_fd) ||
91 !ReadObject(&tmp.y_error, kDoublePackedSize, data_fd)) {
92 goto abort_load;
93 }
94 err_[ErrorIndex(x, y, p)] = tmp;
95 }
96 }
97 }
98
99 fclose(data_fd);
100 return;
101
102 abort_load:
103 x_range_.reset();
104 x_range_len_ = 0;
105 y_range_.reset();
106 y_range_len_ = 0;
107 p_range_.reset();
108 p_range_len_ = 0;
109 err_.reset();
110 fclose(data_fd);
111 }
112
SyncInterpretImpl(HardwareState * hwstate,stime_t * timeout)113 void NonLinearityFilterInterpreter::SyncInterpretImpl(HardwareState* hwstate,
114 stime_t* timeout) {
115 if (enabled_.val_ && err_.get() && hwstate->finger_cnt == 1) {
116 FingerState* finger = &(hwstate->fingers[0]);
117 if (finger) {
118 Error error = GetError(finger->position_x, finger->position_y,
119 finger->pressure);
120 finger->position_x -= error.x_error;
121 finger->position_y -= error.y_error;
122 }
123 }
124 next_->SyncInterpret(hwstate, timeout);
125 }
126
127 NonLinearityFilterInterpreter::Error
LinearInterpolate(const Error & p1,const Error & p2,float percent_p1) const128 NonLinearityFilterInterpreter::LinearInterpolate(const Error& p1,
129 const Error& p2,
130 float percent_p1) const {
131 Error ret;
132 ret.x_error = percent_p1 * p1.x_error + (1.0 - percent_p1) * p2.x_error;
133 ret.y_error = percent_p1 * p1.y_error + (1.0 - percent_p1) * p2.y_error;
134 return ret;
135 }
136
137 NonLinearityFilterInterpreter::Error
GetError(float finger_x,float finger_y,float finger_p) const138 NonLinearityFilterInterpreter::GetError(float finger_x, float finger_y,
139 float finger_p) const {
140 // First, find the 6 values surrounding the point to interpolate over
141 Bounds x_bounds = FindBounds(finger_x, x_range_, x_range_len_);
142 Bounds y_bounds = FindBounds(finger_y, y_range_, y_range_len_);
143 Bounds p_bounds = FindBounds(finger_p, p_range_, p_range_len_);
144
145 if (x_bounds.lo == -1 || x_bounds.hi == -1 || y_bounds.lo == -1 ||
146 y_bounds.hi == -1 || p_bounds.lo == -1 || p_bounds.hi == -1) {
147 Error error = { 0, 0 };
148 return error;
149 }
150
151 // Interpolate along the x-axis
152 float x_hi_perc = (finger_x - x_range_[x_bounds.lo]) /
153 (x_range_[x_bounds.hi] - x_range_[x_bounds.lo]);
154 Error e_yhi_phi = LinearInterpolate(
155 err_[ErrorIndex(x_bounds.hi, y_bounds.hi, p_bounds.hi)],
156 err_[ErrorIndex(x_bounds.lo, y_bounds.hi, p_bounds.hi)],
157 x_hi_perc);
158 Error e_yhi_plo = LinearInterpolate(
159 err_[ErrorIndex(x_bounds.hi, y_bounds.hi, p_bounds.lo)],
160 err_[ErrorIndex(x_bounds.lo, y_bounds.hi, p_bounds.lo)],
161 x_hi_perc);
162 Error e_ylo_phi = LinearInterpolate(
163 err_[ErrorIndex(x_bounds.hi, y_bounds.lo, p_bounds.hi)],
164 err_[ErrorIndex(x_bounds.lo, y_bounds.lo, p_bounds.hi)],
165 x_hi_perc);
166 Error e_ylo_plo = LinearInterpolate(
167 err_[ErrorIndex(x_bounds.hi, y_bounds.lo, p_bounds.lo)],
168 err_[ErrorIndex(x_bounds.lo, y_bounds.lo, p_bounds.lo)],
169 x_hi_perc);
170
171 // Interpolate along the y-axis
172 float y_hi_perc = (finger_y - y_range_[y_bounds.lo]) /
173 (y_range_[y_bounds.hi] - y_range_[y_bounds.lo]);
174 Error e_plo = LinearInterpolate(e_yhi_plo, e_ylo_plo, y_hi_perc);
175 Error e_phi = LinearInterpolate(e_yhi_phi, e_ylo_phi, y_hi_perc);
176
177 // Finally, interpolate along the p-axis
178 float p_hi_perc = (finger_p - p_range_[p_bounds.lo]) /
179 (p_range_[p_bounds.hi] - p_range_[p_bounds.lo]);
180 Error error = LinearInterpolate(e_phi, e_plo, p_hi_perc);
181
182 return error;
183 }
184
185 NonLinearityFilterInterpreter::Bounds
FindBounds(float value,const std::unique_ptr<double[]> & range,size_t len) const186 NonLinearityFilterInterpreter::FindBounds(
187 float value,
188 const std::unique_ptr<double[]>& range,
189 size_t len) const {
190 Bounds bounds;
191 bounds.lo = bounds.hi = -1;
192
193 for (size_t i = 0; i < len; i++) {
194 if (range[i] <= value) {
195 bounds.lo = i;
196 } else {
197 bounds.hi = i;
198 break;
199 }
200 }
201
202 return bounds;
203 }
204
205 } // namespace gestures
206