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